A calibration method, device, apparatus, and storage medium between coordinate systems
By combining a three-dimensional force sensor with the Jacobian matrix and the least squares method, the coordinate transformation matrix between the robot joint and the force sensor is calculated, which solves the complex and costly calibration problem in the existing technology and realizes low-cost and high-precision coordinate system calibration.
Patent Information
- Application Number
- CN202410467797.X
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2024-04-18
- Publication Date
- 2026-11-17
- Estimated Expiration
- 2044-04-18
AI Technical Summary
In existing technologies, the coordinate system calibration process for robot joints and force sensors is complex and costly. Six-dimensional force sensors are expensive and consume a lot of power, making them unsuitable for low-cost designs.
A three-dimensional force sensor is used in conjunction with the Jacobian matrix and the least squares method to obtain the joint torque change value and the sensor detection value through multiple external force applications. The transformation matrix from the three-dimensional force sensor coordinate system to the end joint coordinate system is calculated to simplify the calibration process.
It reduces the cost and power consumption of force sensors in robots, simplifies the calculation process for coordinate system calibration, and improves calibration accuracy.
Smart Images

Figure CN118123840B_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of robotics, and more specifically to a calibration method, device, apparatus, and storage medium for the coordinate system of a robot joint and a force sensor. Background Technology
[0002] In existing technologies, the calibration of the coordinate systems between robot joints and force sensors generally uses a six-dimensional force sensor to calibrate the relationship between the six-dimensional force sensor coordinate system and the end-joint coordinate system.
[0003] However, calibrating the relationship between the sensor coordinate system and the robot joint coordinate system using a six-dimensional force sensor involves a complex calculation process. In addition, six-dimensional sensors are expensive and consume a lot of power, which is not conducive to the low-cost design of robots. Summary of the Invention
[0004] To address the technical problems raised in the background art, the first aspect of the present invention provides a calibration method between the coordinate systems of a robot joint and a force sensor, comprising the following steps:
[0005] S1: The tool connected to the end joint of the robot is subjected to multiple external forces;
[0006] S2: Obtain the torque change value of each joint of the robot under each external force, and obtain the detection value of the three-dimensional force sensor set between the end joint and the tool under each external force.
[0007] S3: Based on the multiple torque change values of each joint, the Jacobian matrix and the robot joint transformation matrix are used to calculate multiple external force values in the end joint coordinate system;
[0008] S4: Using multiple external force values in the end-joint coordinate system and multiple detection values from the three-dimensional force sensor, the transformation matrix from the three-dimensional force sensor coordinate system to the end-joint coordinate system is calculated using the least squares method.
[0009] Secondly, the present invention provides a calibration device for the coordinate system between a robot joint and a force sensor, comprising:
[0010] tool;
[0011] The body has an end connected to the tool. The body includes a plurality of interconnected joints, including an end joint. The end joint is disposed adjacent to the tool relative to the other joints. The body includes a connecting rod and a three-dimensional force sensor. The connecting rod is connected between the end joint and the tool. The three-dimensional force sensor is disposed at the connection between the tool and the connecting rod.
[0012] One or more processors are connected to the three-dimensional force sensor to receive the detection values from the three-dimensional force sensor;
[0013] Memory; and
[0014] One or more computer programs, wherein the one or more computer programs are stored in the memory and configured to be executed by the one or more processors, wherein the processors, when executing the computer programs, implement the steps of the calibration method between the coordinate systems of the robot joints and the force sensors as described above.
[0015] Thirdly, the present invention provides a calibration device for the coordinate system between a robot joint and a force sensor, comprising:
[0016] The tool, connected to the end joint of the robot, is used to receive multiple external forces.
[0017] The data acquisition module is used to acquire the torque change value of each joint under each external force, and to acquire the detection value of the three-dimensional force sensor set between the end joint and the tool under each external force.
[0018] The data calculation module is used to calculate multiple external force values in the end-joint coordinate system based on multiple torque change values of each joint using the Jacobian matrix; and to calculate the transformation matrix from the three-dimensional force sensor coordinate system to the end-joint coordinate system using the multiple external force values in the end-joint coordinate system and multiple detection values of the three-dimensional force sensor using the least squares method.
[0019] Fourthly, the present invention provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of the calibration method between the coordinate systems of a robot joint and a force sensor as described above.
[0020] In the calibration scheme between the robot joint and the force sensor coordinate system of the present invention, the transformation matrix from the force sensor to the robot end joint is calibrated by a three-dimensional force sensor. Compared with the transformation matrix from the force sensor coordinate system to the end joint coordinate system calibrated by a six-dimensional force sensor, the calibration between coordinate systems is achieved by using the torque change values of each joint, the sensor detection values, the Jacobian matrix, and the least squares method. This simplifies the calculation process of the calibration between coordinate systems and reduces the cost and power consumption of the force sensor in the robot. Attached Figure Description
[0021] Figure 1 A flowchart illustrating the calibration method between the coordinate systems of a robot joint and a force sensor, provided for an embodiment of the present invention.
[0022] Figure 2 A schematic diagram of the structure of a calibration device between the coordinate systems of a robot joint and a force sensor, provided for an embodiment of the present invention;
[0023] Figure 3 for Figure 2 An exploded view of the calibration device between the coordinate systems of the robot joints and the force sensor.
[0024] Figure 4 for Figure 2 A partial cross-sectional view of the calibration device between the coordinate systems of the robot joints and the force sensor.
[0025] Figure 5 This is a schematic diagram of the principle structure of the calibration device between the coordinate system of the robot joint and the force sensor provided by the present invention. Detailed Implementation
[0026] To make the technical problems to be solved, the technical solutions, and the beneficial effects of the present invention clearer, the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.
[0027] In this specification and the accompanying drawings, steps and elements that are substantially the same or similar are indicated by the same or similar reference numerals, and repeated descriptions of these steps and elements will be omitted.
[0028] In embodiments of this disclosure, the terms "module" or "unit" refer to a computer program or part of a computer program that has a predetermined function and works with other related parts to achieve a predetermined goal, and can be implemented wholly or partially using software, hardware (such as processing circuitry or memory), or a combination thereof. Similarly, a processor (or multiple processors or memory) can be used to implement one or more modules or units. Furthermore, each module or unit can be part of an overall module or unit that includes the functionality of that module or unit.
[0029] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this disclosure belongs. The terminology used herein is for the purpose of describing embodiments of the invention only and is not intended to limit the invention.
[0030] The terms "first," "second," and similar terms used in this invention do not indicate any order, quantity, or importance, but are merely used to distinguish different components. Similarly, terms such as "comprising" or "including" mean that the element or object preceding the word covers the element or object listed after the word and its equivalents, without excluding other elements or objects. Terms such as "connected" or "linked" are not limited to physical or mechanical connections, but can include electrical connections, whether direct or indirect. Terms such as "upper," "lower," "left," and "right" are used only to indicate relative positional relationships, and these relative positional relationships may change accordingly when the absolute position of the described object changes.
[0031] This invention provides a calibration method (hereinafter referred to as the calibration method) for the coordinate system between a robot joint and a force sensor, a calibration device 100 (hereinafter referred to as the calibration device 100) for the coordinate system between a robot joint and a force sensor, a calibration apparatus 1000 (hereinafter referred to as the calibration apparatus 1000) for the coordinate system between a robot joint and a force sensor, and a storage medium. The calibration device 100 described in this invention can be applied to various types of robots that require coordinate system calibration, such as service robots, industrial robots, special-purpose robots, medical robots, etc. In this embodiment, the calibration device 100 is used as an example of a magnetically controlled robot in a medical robot. It is understood that the calibration device 100 provided by this invention can also be used in fields mentioned above or not mentioned. The calibration method provided in this embodiment can be applied to the calibration device 100 and the calibration apparatus 1000.
[0032] Please refer to Figure 1 The calibration method provided by this invention includes the following steps:
[0033] S1: The tool connected to the end joint of the robot is subjected to multiple external forces;
[0034] S2: Obtain the torque change value of each joint of the robot under each external force, and obtain the detection value of the three-dimensional force sensor set between the end joint and the tool under each external force.
[0035] S3: Based on the multiple torque change values of each joint, the Jacobian matrix and the robot joint transformation matrix are used to calculate multiple external force values in the end joint coordinate system.
[0036] S4: Using multiple external force values in the end-effector coordinate system and multiple detection values from the three-dimensional force sensor, the transformation matrix from the three-dimensional force sensor coordinate system to the end-effector coordinate system is calculated using the least squares method.
[0037] In the calibration scheme of this invention, the transformation matrix from the force sensor to the robot's end joint is calibrated using a three-dimensional force sensor, compared to the transformation matrix from the force sensor coordinate system to the end joint coordinate system calibrated using a six-dimensional force sensor. The calibration between coordinate systems is achieved by utilizing the torque change values of each joint, the sensor's detection values, the Jacobian matrix, and the least squares method. This simplifies the calculation process for calibration between coordinate systems and reduces the cost and power consumption of the force sensor in the robot.
[0038] In step S1, the tool connected to the robot's end effector is involved, and the tool is used to receive multiple external forces.
[0039] Please refer to Figures 2 to 4 The calibration device 100 provided in this embodiment includes a tool 110 and a body 100a, with the tool 110 connected to the end of the body 100a. In this embodiment, the body 110a refers to a robotic arm.
[0040] Tool 110 can vary depending on its application and functional requirements. For example, tool 110 can be an end effector used to perform operations such as grasping, clamping, and placing various objects, and tool 110 can help calibrate device 100 to operate objects accurately. Tool 110 can have various clamps, suction cups, grinding wheels, nozzles, magnetic components, and other parts.
[0041] In some embodiments, the calibration device 100 is applied to a magnetically controlled device. In this embodiment, the tool 110 includes an external magnet 108. Figure 4 The calibration device 100 is used to control the external magnet 108 via the processor 160 to drive the movement of the external controlled magnetic device, that is, the external magnet 108 is used to guide the movement of the controlled magnetic component. In some embodiments, the controlled magnetic component is a capsule endoscope, which has an internal magnet. The external magnet 108 and the internal magnet have mutual magnetic force. The position and orientation of the external magnet 108 in the tool 110 can be changed to drive the movement of the capsule endoscope, thereby realizing the examination of the target area of the biological body.
[0042] Specifically, the body 100a includes a plurality of interconnected joints, among which a terminal joint 140 is disposed relative to the other joints adjacent to the tool 110.
[0043] In this embodiment, the body 100a includes a connecting rod 101 and a three-dimensional force sensor 103. The connecting rod 101 is connected between the end joint 140 and the tool 110, and the three-dimensional force sensor 103 is disposed at the connection between the tool 110 and the connecting rod 101.
[0044] In step S1, the tool 110 is subjected to external forces. Each application of these forces corresponds to a torque change in each joint and a set of detection values from the three-dimensional force sensor 103. Multiple applications of external forces facilitate the acquisition of multiple torque change values and multiple data acquisitions from the three-dimensional force sensor. Using the torque change values from multiple applications of external forces and the detection values from the three-dimensional force sensor for coordinate system calibration improves the accuracy of the coordinate system calibration.
[0045] In some implementations, the external force is applied 100 times or more. In some implementations, the external force is applied 200 times or more.
[0046] The external force received by tool 110 can come from other instruments outside the calibration device 100, such as another robotic arm, or from the operator. The effect of the external force can cause the tool 110 to move and / or change its posture.
[0047] In some embodiments, tool 110 receives external force to achieve robot drag teaching. The coordinate system calibration scheme of this invention is used in drag teaching. After receiving external force, calibration device 100 records and processes the external force information. For example, after receiving an external force of a certain magnitude and / or direction range, calibration device 100 drives tool 110 to move according to predetermined motion rules. It is understood that the coordinate system calibration scheme of this invention can also be used for other three-dimensional force sensors besides drag teaching.
[0048] In step S2, the torque change values of each joint of the robot under each external force are obtained, specifically including:
[0049] S211: Before the external force is applied, record the torque value τ of each joint under static conditions. s0 :{τ 01 , τ 02 , ......, τ 0n}, where n represents the total number of joints of the robot, and n is a positive integer.
[0050] τ s0 :{τ 01 , τ 02 , ......, τ 0n This can be understood as the initial torque value of each joint under static conditions before the application of external force. The torque value of each joint can be calculated by the motor current corresponding to the joint, or it can be directly measured by a dedicated torque sensor.
[0051] In this embodiment, the robot body includes three joints, i.e., n=3. In some embodiments, the robot body includes other numbers of joints, such as 1, 2, 4, 5, 6, etc., which are not limited here.
[0052] S212: Record the static torque value τ of each joint when the external force is applied. sa : {τ1, τ2, ..., τ n}, where a represents the number of times force is applied, a is a positive integer, and a≥100.
[0053] Step S212 records the current torque values of each joint caused by the external force. It is understood that, similar to step S211, the torque values of each joint can be calculated from the corresponding motor current, or a dedicated torque sensor can be used to directly measure the torque values of the corresponding joints. Each application of external force corresponds to a τ value. sa : {τ1, τ2, ..., τ n}
[0054] S213: Calculate the torque change value Δτ of each joint under the action of the external force according to Formula 1. sa : {Δτ1, Δτ2, ..., Δτ n},
[0055] Δτ sa : {Δτ1, Δτ2, ..., Δτ n}=T sa : {τ1, τ2, ..., τ n}-τ s0 :{τ 01 , τ 02 , ......, τ 0n}
[0056] (Formula 1).
[0057] Under each external force, a torque change value Δτ can be calculated using Formula 1. sa Under the action of multiple external forces, multiple torque change values Δτ can be obtained, each corresponding to one of the multiple external forces. sa .
[0058] Steps S211-S213 are used to obtain the torque change values of each joint of the robot under static conditions, that is, the initial torque value of each joint and the torque value of each joint under the action of external force are obtained when the robot is stationary. In the calculation process, it is not necessary to establish the robot dynamics equations, and the calculation process is relatively simple.
[0059] The following steps S221-10230 are used to obtain the torque change values of each joint of the robot under dynamic conditions, that is, the initial torque value of each joint, and the torque value of each joint under the action of external force are obtained in the state of robot motion.
[0060] S221: Establish the dynamic equations of the articulated robot.
[0061] Specifically, the articulated robot has n joints, where n is a positive integer, and the dynamic equation is represented by the following formula 7:
[0062]
[0063] In the formula: τ∈R n It is the control torque input vector, τ fi ∈R n It is the torque vector of joint friction force.
[0064] It is the vector of centrifugal force and Coriolis force, G(q)∈R n It is the gravity vector; M(q)∈R n It is the mass matrix, q∈R n It is the joint position vector.
[0065] S222: Linearize the aforementioned dynamic equations;
[0066] The linear simplification yields Formula 8:
[0067]
[0068] In the formula, τ∈R n Control torque input vector, It is the identification matrix or observation matrix, θ s ∈R 12n These are inertial parameters, and the inertial parameters of the connecting rod between any two joints, along with the joint friction coefficient, are composed as shown in Formula 9:
[0069] θ s =[I xx I xy I xz I yy I yz I zz [Mx, My, Mz, M, fv, fc] (Formula 9)
[0070] Where fv and fc are terms related to joint friction;
[0071] Equation 10 is the modeling equation for friction:
[0072]
[0073] Among them The angular velocity of the joint;
[0074]
[0075] By simplifying the inertial parameters in Equation 8 into the minimum lumped inertial parameters, we obtain Equation 12:
[0076]
[0077] in It is the coefficient matrix with minimal parameters. It is the minimum set of parameters; It is the equation for calculating torque.
[0078] S223: The robot operates using an optimized excitation trajectory, and the specified position, velocity, acceleration, and torque values of each joint are collected;
[0079] S224: Using the specified position, velocity, acceleration and torque values of each joint obtained in step S223, the least squares method is used to identify the dynamic parameters and obtain the dynamic parameters;
[0080] The identification of dynamic parameters using the least squares method is specifically performed using Formula 13:
[0081]
[0082] In the formula It is a matrix composed of the specified position, velocity, and acceleration of the joint collected in step S223, where τ is the joint torque value collected in step S223;
[0083] S225: Start and run the robot;
[0084] S226: When the tool is subjected to external force, the real-time position, velocity, acceleration, and torque value τ of each joint are collected. sa : {τ1, τ2, ..., τ n}, where a represents the number of times force is applied, a is a positive integer, and a≥100;
[0085] S227: Using the dynamic parameters and the real-time position, velocity, and acceleration of each joint under the action of external force collected in step S226, calculate the real-time torque value τ of each joint under dynamic conditions. s0 :{τ 01 , τ 02 , ......, τ 0n};
[0086] S228: Calculate the torque change value of each joint under the action of the external force.
[0087] Δτ sa : {Δτ1, Δτ2, ..., Δτ n},
[0088] Δτ sa : {Δτ1, Δτ2, ..., Δτ n}=τ sa : {τ1, τ2, ..., τ n}-τ s0 :{τ 01 , τ 02 , ......, τ 0n}
[0089] (Formula 1).
[0090] Step S2 is used to obtain the torque change value and the detection value of the three-dimensional force sensor 103, in the calibration device 1000 ( Figure 5 In this process, data acquisition module 1300 is used to acquire the torque change values of each joint under each external force application, and to acquire the detection values of the three-dimensional force sensor installed between the end joint and the tool under each external force application. In some embodiments, data acquisition module 1300 is used to perform steps S211-S213. In some embodiments, data acquisition module is used to perform steps S221-S230.
[0091] The data acquisition module 1300 includes a three-dimensional force sensor 103, thereby acquiring the detection values of the three-dimensional force sensor 103. In some embodiments, the data acquisition module 1300 also includes a torque sensor for reading the torque value of the corresponding joint. In some embodiments, the torque sensor can also directly acquire the torque change value of the corresponding joint. In some embodiments, the data acquisition module 1300 includes a calculation unit for calculating the torque change value based on the torque values before and after the application of the external force and Formula 1. In some embodiments, the calculation unit is used to calculate the torque change value of the corresponding joint based on the motor current corresponding to the joint.
[0092] Step S3 specifically includes:
[0093] S31: Calculate the external force F in the world coordinate system using Formula 2. wa ,
[0094]
[0095] Wherein, matrix J is the Jacobian matrix of the distal joint:
[0096]
[0097] Matrix J can be obtained through robot transformation using the robot DH parameter method. In matrix J, X, Y, and Z are the coordinate values of tool 110, i.e., tool 110 position P: {X, Y, Z}; q1, q2, q n For the angles of each joint. Matrix J T The transpose of matrix J, matrix Let J be the inverse matrix of matrix JT.
[0098] S32: F in Formula 2 wa External force values converted to the end-joint coordinate system.
[0099] In step S32, the external force value F in the end-joint coordinate system is calculated according to formula 3. jointa ,
[0100]
[0101] in, This is the robot joint transformation matrix, specifically the transformation matrix from the world coordinate system to the end-joint coordinate system, which can be calculated based on the robot coordinate DH parameters.
[0102] According to steps S31-S32, the torque change value Δτ of each joint is used. sa The external force F in the world coordinate system is calculated from the Jacobian matrix J of the end joint. wa (Including the magnitude and direction information of external forces), and then using the transformation matrix from the world coordinate system to the end joint coordinate system. The external force value F in the world coordinate system wa Transform to the end-joint coordinate system to obtain the external force value F in the end-joint coordinate system. jointa External force F in the end-joint coordinate system jointa This includes information on the magnitude and direction of external forces in the end-joint coordinate system.
[0103] In step S4, the transformation matrix from the three-dimensional force sensor coordinate system to the end joint coordinate system is calculated according to formula 4.
[0104]
[0105] Among them, F joint The external force F in the end-joint coordinate system calculated by Formula 3 jointa The set of numbers, F sensora It is a set of detection values from a three-dimensional force sensor in the sensor coordinate system.
[0106] Specifically, the calibration device 100 is equipped with m three-dimensional force sensors at its end, where m is a positive integer. In some embodiments, m = 1, and in some embodiments, m ≥ 2.
[0107] In some implementations, multiple three-dimensional force sensors operate simultaneously, thereby avoiding inaccurate detection values caused by damage to some three-dimensional force sensors; under each force application, each three-dimensional force sensor is used to transmit the collected detection value to the processor.
[0108] F in Formula 4 sensora Specifically, it is expressed as follows:
[0109]
[0110] in,
[0111] F 1sensorx F 1sensory F 1sensorz1 This is the first three-dimensional force sensor; the detected value of the three-dimensional force sensor when the first external force is applied.
[0112] F 1sensorxa F 1sensorya F 1sensorza For the first three-dimensional force sensor, the detected value of the three-dimensional force sensor when the a-th external force is applied is...
[0113] F msensorx1 F msensory F msensorz1 Let m be the m-th three-dimensional force sensor, and let m be the detected value of the three-dimensional force sensor when the first external force is applied.
[0114] F msensorxa F msensorya F msensorza Let m be the m-th three-dimensional force sensor, and let a be the detected value of the three-dimensional force sensor when the a-th external force is applied.
[0115] In Formula 3, F jointa Represented as F jointa :{F jointxa F jointya F jointza},
[0116] F in Formula 4 joint Represented as: Right now
[0117]
[0118] It is understandable that in other implementations, F joint It can be F jointa Other forms of expression for number sets, F sensoraIt can be any other representation of the detection values, and there are no restrictions here.
[0119] In calibration device 1000 ( Figure 5 In this process, steps S3-S4 are executed by the data calculation module 1500. Specifically, the data calculation module 1500 calculates multiple external force values in the end-joint coordinate system based on multiple torque variation values of each joint using the Jacobian matrix; and uses the multiple external force values in the end-joint coordinate system and multiple detection values from the three-dimensional force sensor to calculate the transformation matrix from the three-dimensional force sensor coordinate system to the end-joint coordinate system using the least squares method. It is understood that the data calculation module 1500 is also used to execute steps S31-S32.
[0120] In some implementations, step S4 is followed by:
[0121] S5: Under arbitrary external force, based on the detection value of the three-dimensional force sensor and the transformation matrix from the three-dimensional force sensor coordinate system to the end joint coordinate system... and the transformation matrix from the end-joint coordinate system to the world coordinate system. Calculate the external force value in the world coordinate system, and adjust and control the parameters of the end joint that drive the tool to move based on the external force value in the world coordinate system.
[0122] Specifically, following steps S1-S4, the transformation matrix from the three-dimensional force sensor coordinate system to the end-joint coordinate system is calculated. After the calibration equipment 100 is officially started and running, based on step S5, when the tool 110 receives any external force, the external force value in the world coordinate system can be calculated. Then, based on the external force value in the world coordinate system, the motion parameters of the end effector joint can be adjusted and controlled, thereby adjusting and controlling the parameters that drive the tool's movement according to preset rules. The calculated external force value in the world coordinate system includes both the magnitude and direction of the external force in the world coordinate system.
[0123] Step S5 specifically includes the following steps:
[0124] S51: Obtain the number of three-dimensional force sensors F sensor ,
[0125] F sensor
[0126] =[F 1sensorx F 1sensory F 1sensorz … F msensorx F msensory F msensorz ] T
[0127] S52: Calculate the external force F in the joint coordinate system joint ,
[0128]
[0129] S53: Calculate the external force F acting in the world coordinate system w ,
[0130]
[0131] In the formula This represents the transformation matrix from the joint coordinate system to the world coordinate system. for The transpose, i.e.
[0132] S54: Based on the external force value in the world coordinate system, adjust and control the motion parameters of the end joint, and then adjust and control the parameters that drive the tool to move according to preset rules.
[0133] Calibration device 1000 between the coordinate systems of robot joints and force sensors Figure 5 In step S5, the data calculation module 1500 executes the process. That is, when tool 110 is subjected to any external force, the data calculation module 1500 calculates the value detected by the three-dimensional force sensor 103 and the transformation matrix from the force sensor coordinate system to the end-joint coordinate system. and the transformation matrix from the end-joint coordinate system to the world coordinate system. The external force value in the world coordinate system is calculated, and the parameters for the end joint to drive the tool 110 to move are adjusted and controlled based on the external force value in the world coordinate system. It is understood that the data calculation module 1500 is also used to execute steps S51-S54.
[0134] like Figure 2 As shown, the calibration device 100 provided in this application includes a memory 150, one or more processors 160, and one or more computer programs. The one or more computer programs are stored in the memory 150 and configured to be executed by one or more processors 160, which, when executing the computer programs, implement the steps of the calibration method described above. Figure 2 The exemplary location of the memory 150 and the processor 160 is shown in the figure. In other embodiments, the memory 150 and the processor 160 may be located in other locations of the calibration device 100. The specific location of the memory 150 and the processor 160 is not limited in this application.
[0135] The calibration device 100 provided in this embodiment further includes a tool 110 and a body 100a, with the tool 110 connected to the end of the body 100a. In this embodiment, the body 110a refers to a robotic arm.
[0136] Tool 110 can vary depending on its application and functional requirements. For example, tool 110 can be an end effector used to perform operations such as grasping, clamping, and placing various objects, and tool 110 can help calibrate device 100 to operate objects accurately. Tool 110 can have various clamps, suction cups, grinding wheels, nozzles, magnetic components, and other parts.
[0137] like Figure 4 As shown, in this embodiment, tool 110 includes an external magnet 108 for guiding the movement of a controlled magnetic component. In some embodiments, the controlled magnetic component is a capsule endoscope, which has an internal magnet. The external magnet 108 and the internal magnet have a mutual magnetic force interaction. The position and orientation of the external magnet 108 in tool 110 can be changed to drive the capsule endoscope to move, thereby enabling the examination of target areas of a biological organism.
[0138] like Figure 2 As shown, the body 100a includes a plurality of interconnected joints, among which a distal joint 140 is disposed adjacent to the tool 110 relative to the other joints. In this embodiment, the body 100a includes three joints: a first joint 120, a second joint 130, and a distal joint 140. The first joint 120, the second joint 130, and the distal joint 140 are disposed at intervals along the extending direction of the body 100a. The distal joint 140 is disposed adjacent to the tool 110 relative to the other joints, and further, the tool 110 is directly connected to the distal joint 140.
[0139] Please combine Figure 2 See Figure 3 and Figure 4 The body 100a includes a connecting rod 101, which connects the end joint 140 and the tool 110. In some embodiments, the connecting rod 101 is omitted between the end joint 140 and the three-dimensional force sensor 140, that is, the tool 110 is directly connected to the end joint 140.
[0140] Optionally, one end of the connecting rod 101 is rotatably connected to the end joint 140 of the calibration device 100, and the end of the connecting rod 101 away from the end joint 140 is fixedly connected to the tool 110.
[0141] like Figure 2 and Figure 4As shown, in this embodiment, the connecting rod 101 and the end joint 140 have two connection points. The line connecting the two connection points is the rotation axis 1011 of the connecting rod 101. The connecting rod 101 rotates around the rotation axis 1011 within a preset angle range, so that the posture of the tool 110 is flexible and controllable, meeting more usage scenarios. The preset angle range can be within ±30° relative to the horizontal plane.
[0142] In some embodiments, the calibration device 100 keeps the connecting rod 101 in a vertically downward position during operation. For example, during the capsule endoscopy examination, the tool 110 is always kept in a vertical position, which can keep the magnitude and direction of the magnetic force exerted by the tool 110 on the capsule endoscopy consistent, reducing the operational complexity of the examination process.
[0143] like Figure 2 and Figure 3 As shown, the three-dimensional force sensor 103 is disposed at the connection between the tool 110 and the connecting rod 101. The calibration device 100 also includes one or more processors 160, which are connected to the three-dimensional force sensor 103 and are used to receive the detection values of the three-dimensional force sensor 103. In some embodiments, the calibration device 100 is provided with multiple three-dimensional force sensors 103. The multiple three-dimensional force sensors 103 can be all disposed at the connection between the tool 110 and the connecting rod 101, or they can be distributed at different joints.
[0144] Tool 110 includes a housing 105, a motion mechanism 107, and an external magnet 108, with the motion mechanism 107 connected to the external magnet 108. The housing 105 includes an upper protective cover 105a and a lower protective cover 105b that interlock, with the upper protective cover 105a fastened to the top of the lower protective cover 105b. The upper and lower protective covers 105a and 105b interlock to form a closed cavity for accommodating the motion mechanism 107 and the external magnet 108. A three-dimensional force sensor 103 is disposed within the housing 105 and thus housed within this closed cavity. The end of the connecting rod 101 away from the end joint 140 extends into the closed cavity within the housing 105.
[0145] The outer shell 105 is fixed to the top surface of the three-dimensional force sensor 103, and at least one other surface of the three-dimensional force sensor 103 is fixed to the connecting rod 101. The outer shell 105 and the connecting rod 101 are spaced apart. When the outer shell 105, which consists of the upper protective cover 105a and the lower protective cover 105b, is subjected to an external force, the external force is transmitted through the outer shell 105 to the top surface of the three-dimensional force sensor 103. Since the outer shell 105 and the connecting rod 101 are spaced apart, the outer shell 105 will not contact the connecting rod 101 when subjected to force, and thus will not transmit the external force to the connecting rod 101. The outer shell 105 causes the top surface of the three-dimensional force sensor 103 to deform. At least one other surface of the three-dimensional force sensor 103 is fixed to the surface of the connecting rod 101. The three-dimensional force sensor 103 generates an electrical signal corresponding to the external force through the small strain between its different surfaces and forms a detection value. The three-dimensional force sensor 103 transmits the detection value to the processor 160. The processor 160 can obtain the magnitude and direction data of the external force in the coordinate system of the three-dimensional force sensor by receiving the detection value.
[0146] The housing 105 and the connecting rod 101 are spaced apart. Specifically, the upper protective cover 105a has a through hole, through which the connecting rod 101 extends into the enclosed cavity and connects to the motion mechanism 107. A predetermined gap is maintained between the upper protective cover 105a and the connecting rod 101, that is, a predetermined gap is maintained between the edge of the through hole of the upper protective cover 105a and the connecting rod 101. This predetermined range can be between 1-3 mm, including 1 mm and 3 mm. In this way, when the upper protective cover 105a and the lower protective cover 105b are subjected to external forces and undergo slight displacement, the upper protective cover 105a does not contact the connecting rod 101, thus not affecting the detection accuracy of the three-dimensional force sensor 103.
[0147] In some embodiments, the lower protective cover 105b has a partially perforated or hollowed-out structure, which can reduce weight or be used for medical applications such as light exposure and heat transfer.
[0148] like Figure 3 and Figure 4 As shown, in this embodiment, the tool 110 includes an assembly 104, which is spaced apart from the connecting rod 101. The housing 105 is fixed to the assembly 104, and the assembly 104 is fixed to the top surface of the three-dimensional force sensor 103. The assembly 104 is arranged around the side wall of the connecting rod 101 and can be in the form of a sheet or a strip, with its edge extending in a direction that matches the side wall of the assembly 104. In this embodiment, the assembly 104 is annularly fitted onto the radial outer periphery of the connecting rod 101.
[0149] The outer casing 105 is fixed to the assembly 104. In one embodiment, the upper protective cover 105a is fixed to the top surface of the assembly 104, and the lower protective cover 105b is fixed to the bottom surface of the assembly 104. In some embodiments, the upper protective cover 105a is fixed to both the top and bottom surfaces of the assembly 104, and the lower protective cover 105b is interlocked with the upper protective cover 105a. The lower protective cover 105b and the upper protective cover 105a can also be fixed to each other by adhesive, threaded connection, screws, or other fasteners.
[0150] In some embodiments, tool 110 is not fitted with accessory 104, and housing 105 contacts and is fixed to the top surface of three-dimensional force sensor 103.
[0151] In this embodiment, the connecting rod 101 includes a base 101a and a fixing seat 102 that are fixedly connected to each other. The bottom surface and / or side surface of the three-dimensional force sensor 103 are fixed to the fixing seat 102. The end of the base 101a away from the end joint 140 is connected to the motion mechanism 107. The side surface of the base 101a is fixedly connected to the fixing seat 102, which can be fixed by screws or the like, or the base 101a can be integrally formed with the fixing seat 102.
[0152] The motion mechanism 107 is connected to the external magnet 108 and is used to drive the external magnet 108 to move. The calibration device 100 is used to control the external magnet 108 to drive the movement of the external controlled magnetic device through the processor 160.
[0153] The motion mechanism 107 is disposed within the cavity formed by the upper protective cover 105a and the lower protective cover 105b. The motion mechanism 107 is fixedly connected to the end of the connecting rod 101 away from the end joint 140, and the motion mechanism 107 does not contact the upper protective cover 105a and the lower protective cover 105b. Furthermore, the motion mechanism 107 is a magnetically controlled device capable of omnidirectional rotation around horizontal and vertical axes.
[0154] The motion mechanism 107 is only fixedly connected to the connecting rod 101 and the external magnet 108. The vibration generated by the motion mechanism 107 during operation and its own weight are entirely borne by the connecting rod 101, which will not interfere with the detection accuracy of the three-dimensional force sensor 103. Under normal conditions, the three-dimensional force sensor 103 only bears the weight of the assembly 104, the upper protective cover 105a, and the lower protective cover 105b.
[0155] Therefore, the external force measurement accuracy of the calibration device 100 is not affected by the end load of the robotic arm 1 (such as the motion mechanism 107 and the external magnet 108). The three-dimensional force sensor 103 actually only bears the weight of the housing 105 and the assembly 104. Therefore, the three-dimensional force sensor 103 can be a small-range, high-precision model, which improves the detection accuracy, enhances the reliability, reduces the safety risk, and has a small and compact structure with low cost.
[0156] When the revelation function is activated, the tool 110 of the calibration device 100 is used to receive external force, and the three-dimensional force sensor 103 of the calibration device 100 is used to monitor the force applied to the tool 110 in real time and transmit the magnitude and direction information of the force to the processor 160 in the form of a detection value. The processor 160 controls the coordinated movement of each joint according to the magnitude and direction information of the force, thereby driving the tool 110 to move.
[0157] Specifically, when activating the revelation function, the operator or external device only needs to gently drag the outer casing 105 of the tool 110 in the direction required for movement. The three-dimensional force sensor 103 monitors the force applied to the outer casing 105 in real time and transmits the magnitude and direction information of the force to the processor through the detection value. The processor uses the coordinate system calibration method provided in this application to convert the magnitude and direction information of the external force from the sensor coordinate system to the world coordinate system. Then, the processor controls the various joints to move in coordination, thereby adjusting and controlling the parameters of the end joint that drive the tool to move.
[0158] The calibration device 100 provided by this invention is lightweight and sensitive to operation, unaffected by the load size of the robotic arm, and has low cost and high reliability.
[0159] Please refer to Figure 5 A schematic diagram of the calibration device 1000 of the present invention is shown. The calibration device 1000 includes:
[0160] Tool 110 is connected to the end joint of the robot and is used to receive multiple external forces.
[0161] The data acquisition module 1300 is used to acquire the torque change values of each joint of the robot under each external force, and to acquire the detection values of the three-dimensional force sensor set between the end joint and the tool under each external force.
[0162] The data calculation module 1500 is used to calculate multiple external force values in the end-joint coordinate system based on multiple torque change values of each joint using the Jacobian matrix; and to calculate the transformation matrix from the three-dimensional force sensor coordinate system to the end-joint coordinate system using the multiple external force values in the end-joint coordinate system and the multiple detection values of the three-dimensional force sensor using the least squares method.
[0163] This application also provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of the calibration method between the coordinate system of the robot joint and the force sensor as described above.
[0164] Those skilled in the art will understand that all or part of the steps in the various methods of the above embodiments can be implemented by a program instructing related hardware. The program can be stored in a computer-readable storage medium, which may include: read-only memory (ROM), random access memory (RAM), disk or optical disk, etc.
[0165] The accompanying drawings of this invention only show structures relevant to embodiments of the invention; other structures can be referenced from conventional designs.
[0166] It should be noted that, without conflict, the specific technical solutions of the various embodiments of the present invention can be combined to obtain new embodiments.
[0167] It should be noted that the flowcharts and block diagrams in the accompanying drawings illustrate the architecture, functionality, and operation of possible implementations of apparatus, methods, and computer program products according to various embodiments of this disclosure. In this regard, each block in a flowchart or block diagram may represent a module, segment, or portion of code containing at least one executable instruction for implementing a specified logical function. It should also be noted that in some alternative implementations, the functions indicated in the blocks may occur in a different order than those indicated in the drawings. For example, two consecutively indicated blocks may actually be executed substantially in parallel, and they may sometimes be executed in reverse order, depending on the functions involved. It should also be noted that each block in the block diagrams and / or flowcharts, and combinations of blocks in the block diagrams and / or flowcharts, can be implemented using a dedicated hardware-based system that performs the specified function or operation, or using a combination of dedicated hardware and computer instructions.
[0168] In general, the various exemplary embodiments of this disclosure can be implemented in hardware or dedicated circuitry, software, firmware, logic, or any combination thereof. Some aspects can be implemented in hardware, while others can be implemented in firmware or software that can be executed by a controller, microprocessor, or other computing device. When aspects of embodiments of this disclosure are illustrated or described as block diagrams, flowcharts, or using some other graphical representation, it will be understood that the blocks, apparatuses, systems, techniques, or methods described herein can be implemented as non-limiting examples in hardware, software, firmware, dedicated circuitry or logic, general-purpose hardware or controllers or other computing devices, or some combination thereof.
[0169] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included within the protection scope of the present invention.
Claims
1. A calibration method between the coordinate systems of a robot joint and a force sensor, characterized in that, Includes the following steps: S1: The tool connected to the end joint of the robot is subjected to multiple external forces; S2: Obtain the torque change value of each joint of the robot under each external force, and obtain the detection value of the three-dimensional force sensor set between the end joint and the tool under each external force. S3: Based on the multiple torque change values of each joint, using the Jacobian matrix and the robot joint transformation matrix, multiple external force values in the end-joint coordinate system are calculated. Step S3 specifically includes: S31: Calculate the external force values in the world coordinate system using Formula 2. , Formula 2, where the matrix The Jacobian matrix of the end joint is: Wherein, the tool position P: {X, Y, Z} For the angles of each joint; S32: Convert the external force values in Formula 2 to the end-joint coordinate system, and calculate the external force values in the end-joint coordinate system according to Formula 3. , Formula 3; where, This is the transformation matrix from the world coordinate system to the end-joint coordinate system; S4: Using multiple external force values in the end-effector coordinate system and multiple detection values from the three-dimensional force sensor, the transformation matrix from the three-dimensional force sensor coordinate system to the end-effector coordinate system is calculated using the least squares method. The transformation matrix from the three-dimensional force sensor coordinate system to the end-effector coordinate system is then calculated according to Formula 4. , Formula 4; where, The external force value in the end-joint coordinate system calculated by Formula 3. The set of numbers, It is the set of detection values of the three-dimensional force sensor in the sensor coordinate system. The robot end effector is equipped with m three-dimensional force sensors, where m is a positive integer. (Formula 4) Specifically, it is expressed as follows: , in, This is the first three-dimensional force sensor; the detected value of the three-dimensional force sensor when the first external force is applied. For the first three-dimensional force sensor, the detected value of the three-dimensional force sensor when the a-th external force is applied is... Let m be the m-th three-dimensional force sensor, and let m be the detected value of the three-dimensional force sensor when the first external force is applied. Let m be the m-th three-dimensional force sensor, and let a be the detected value of the three-dimensional force sensor when the a-th external force is applied. In formula 3 Represented as ; Formula 4 Represented as: ,Right now .
2. The calibration method between coordinate systems as described in claim 1, characterized in that, Step S2, which involves obtaining the torque change values of each joint of the robot under each external force, specifically includes: S211: Before the external force is applied, record the torque values of each joint under static conditions. , where n represents the total number of joints in the robot, and n is a positive integer; S212: Record the torque values of each joint under static conditions when the external force is applied. , where a represents the number of times force is applied, a is a positive integer, and a≥100; S213: Calculate the torque change value of each joint under the action of the external force according to Formula 1. , , Formula 1.
3. The calibration method between coordinate systems as described in claim 1, characterized in that, Step S2 specifically includes: S221: Establish the dynamic equations of the articulated robot; S222: Linearize the aforementioned dynamic equations; S223: Collect the specified position, velocity, acceleration, and torque of each joint; S224: Using the specified position, velocity, acceleration and torque of each joint obtained in step S223, the dynamic parameters are identified by the least squares method. S225: Start and run the robot; S226: When the tool is subjected to the external force, it collects the real-time position, velocity, acceleration, and torque of each joint. , where a represents the number of times force is applied, a is a positive integer, and a≥100; S227: Using the dynamic parameters and the real-time position, velocity, and acceleration of each joint under the action of external force collected in step S226, calculate the real-time torque value of each joint under dynamic conditions. , where n represents the total number of joints in the robot, and n is a positive integer; S228: Calculate the torque change value of each joint under the action of the external force. , , Formula 1.
4. The calibration method between coordinate systems as described in any one of claims 1-3, characterized in that, Following step S4: S5: Under any external force, calculate the external force value in the world coordinate system based on the detection value of the three-dimensional force sensor, the transformation matrix from the three-dimensional force sensor coordinate system to the end joint coordinate system, and the transformation matrix from the end joint coordinate system to the world coordinate system. Based on the external force value in the world coordinate system, adjust and control the parameters of the end joint that drive the tool to move.
5. A calibration device for the coordinate system between a robot joint and a force sensor, characterized in that, include: tool; The body has an end connected to the tool. The body includes a plurality of interconnected joints, including an end joint. The end joint is disposed adjacent to the tool relative to the other joints. The body includes a connecting rod and a three-dimensional force sensor. The connecting rod is connected between the end joint and the tool. The three-dimensional force sensor is disposed at the connection between the tool and the connecting rod. One or more processors are connected to the three-dimensional force sensor to receive the detection values from the three-dimensional force sensor; Memory; as well as One or more computer programs, wherein the one or more computer programs are stored in the memory and configured to be executed by the one or more processors, wherein the processors, when executing the computer programs, implement the steps of the calibration method between the coordinate systems of a robot joint and a force sensor as described in any one of claims 1 to 4.
6. The calibration device between coordinate systems as described in claim 5, characterized in that, The tool includes a housing, with one end of the connecting rod away from the end joint extending into the housing. A three-dimensional force sensor is disposed within the housing, and the housing is fixed to the top surface of the three-dimensional force sensor. At least one other surface of the three-dimensional force sensor is fixed to the connecting rod. The housing and the connecting rod are spaced apart. When an external force is applied to the housing, the three-dimensional force sensor is used to transmit the detected value to the processor.
7. The calibration device between coordinate systems as described in claim 6, characterized in that, The tool includes an assembly, which is spaced apart from the connecting rod. The housing is fixed to the assembly, and the assembly is fixed to the top surface of the three-dimensional force sensor. The connecting rod includes a base and a fixing seat that are fixedly connected to each other. The bottom surface and / or side surface of the three-dimensional force sensor are fixed to the fixing seat. The tool includes a motion mechanism, and the end of the base away from the end joint is connected to the motion mechanism.
8. The calibration device between coordinate systems as described in any one of claims 5-7, characterized in that, The tool includes an external magnet. The calibration device between the coordinate system of the robot joint and the force sensor is applied to the magnetic control device. The calibration device between the coordinate system of the robot joint and the force sensor is used to control the external magnet to drive the movement of the external controlled magnetic device through the processor.
9. A calibration device employing the calibration equipment between coordinate systems as claimed in claim 5, characterized in that, include: The tool, connected to the end joint of the robot, is used to receive multiple external forces. The data acquisition module is used to acquire the torque change value of each joint under each external force, and to acquire the detection value of the three-dimensional force sensor set between the end joint and the tool under each external force. The data calculation module is used to calculate multiple external force values in the end joint coordinate system based on multiple torque change values of each joint and using the Jacobian matrix. It is used to calculate the transformation matrix from the three-dimensional force sensor coordinate system to the end joint coordinate system by using multiple external force values in the end joint coordinate system and multiple detection values of the three-dimensional force sensor, and employing the least squares method.
10. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by the processor, it implements the steps of the calibration method between the coordinate systems of the robot joint and the force sensor as described in any one of claims 1 to 4.
Citation Information
Patent Citations
Tail-end load compensating method dragged by multi-degree-of-freedom cooperation robot
CN110666794A
Method of high precision force feedback of mechanical arm in stationary or low speed working condition
CN111618859A
