Spring torque compensation method and device, electronic device and storage medium

Abstract

The application relates to a spring torque compensation method and device, an electronic device and a storage medium, wherein the method is suitable for a spring torque compensation system, and steps of the method comprise the following: establishing a world coordinate system based on master hand kinematics; determining a position vector formed by a joint shaft center to a rotating shaft center according to the world coordinate system and structural parameters of the master hand; determining a force vector formed by the rotating shaft center to a lower cut point on a connecting line and a turning device according to the world coordinate system and the structural parameters of the master hand; and determining a compensation torque on the joint shaft according to the position vector and the force vector by using a vector algorithm. Through the application, the problem that a compensation torque is wrong due to the need for artificial judgment of the positive and negative of a joint angle and subsequent calculation is solved, the size and direction of the compensation torque on the joint shaft are accurately calculated according to the position vector and the force vector, and torque compensation errors are avoided.

Description

Technical Field

[0001] This application relates to the field of robotics, and in particular to spring torque compensation methods, apparatus, electronic devices, and storage media. Background Technology

[0002] Robotic minimally invasive surgery not only inherits the advantages of traditional minimally invasive surgery—smaller incisions, less pain, and faster recovery—but also offers more flexible and precise operation. The master hand, as the interface between the user and the minimally invasive surgical robot, transmits the user's actions to the slave hand, controlling the slave robotic arm to complete the surgical procedure. Simultaneously, it feeds back the interaction forces between the slave robotic arm and the patient's tissues to the user. During the user's operation of the master hand for master-slave control of minimally invasive surgery, it is desirable that the weight of the master hand's linkages be fully compensated to avoid fatigue and discomfort.

[0003] Due to the inherent design characteristics of the master hand's mechanism, the output torque of the master hand's joint motor is often insufficient, requiring passive compensation methods. These include using counterweights or springs to offset part of the weight of the master hand's connecting rods, thereby improving the efficiency of the motor's output torque. However, using counterweights to compensate for the connecting rod's weight significantly increases the overall weight and inertia of the master hand. Spring-based compensation, on the other hand, does not increase the overall weight and inertia of the master hand. However, spring-based compensation requires manual determination of the joint angle's sign for subsequent calculations; incorrect determination can lead to errors in the compensation torque.

[0004] There is currently no effective solution to the problem that related technologies require manual judgment of the sign of the joint angle for subsequent calculations, and that incorrect judgment will lead to errors in the compensation torque. Summary of the Invention

[0005] This embodiment provides a spring torque compensation method, device, electronic device, and storage medium to solve the problem in related technologies that require manual judgment of the sign of the joint angle for subsequent calculations, which can lead to errors in the compensation torque if the judgment is incorrect.

[0006] Firstly, this embodiment provides a spring torque compensation method applicable to a spring torque compensation system. The spring torque compensation system includes a fixed column, a turntable, a connecting rod, a joint shaft, a rotating shaft, a steering device, a spring, and a connecting line. The fixed column is located at any point above and outside the turntable. The steering device is located at any point below the turntable. One end of the spring is connected to the fixed column, and the other end of the spring is connected to one end of the connecting line. The other end of the connecting line is connected to the rotating shaft via the steering device. The connecting rod is mounted on the turntable with the joint shaft as a fixed point. The rotating shaft rotates along the turntable. The method includes:

[0007] Establish a world coordinate system based on the kinematics of the dominant hand;

[0008] Based on the world coordinate system and the structural parameters of the main hand, determine the position vector formed by the center of the joint axis and the center of the rotation axis;

[0009] Based on the world coordinate system and the structural parameters of the main hand, determine the force vector formed by the line from the center of the rotation axis to the point of tangency on the steering device.

[0010] The compensating torque on the joint axis is determined based on the position vector and the force vector.

[0011] In some embodiments, establishing a world coordinate system based on the kinematics of the dominant hand includes:

[0012] A world coordinate system is established with the intersection of the joint axis and the turntable in the main hand as the center, the straight line passing through the center of the joint axis and the center of the rotation axis as the X-axis, the direction from the center of the joint axis to the center of the rotation axis as the direction of the X-axis, the straight line passing through the center of the joint axis and perpendicular to the X-axis as the Y-axis, and the direction from the center of the joint axis to the rotation axis as the direction of the Y-axis.

[0013] In some embodiments, the center of the joint axis coincides with the center of the turntable.

[0014] In some embodiments, determining the position vector formed by the joint axis center to the rotation axis center based on the world coordinate system and the structural parameters of the master hand includes:

[0015] Based on the world coordinate system and the structural parameters of the main hand, determine the coordinates of the center of the joint axis and the center of the rotation axis;

[0016] Based on the coordinates of the center of the joint axis and the center of the rotation axis, determine the position vector formed by the distance from the center of the joint axis to the center of the rotation axis.

[0017] In some embodiments, determining the force vector formed by the line connecting the center of the rotation axis to the point of tangency on the steering device, based on the world coordinate system and the structural parameters of the main hand, includes:

[0018] Based on the world coordinate system and the structural parameters of the master hand, determine the coordinates of the center of the steering device.

[0019] Using the formula for the external tangent point of a circle, the coordinates of the lower tangent point of the connecting line and the steering device are determined based on the coordinates of the center of the rotating shaft, the coordinates of the center of the steering device, and the radius.

[0020] Based on the coordinates of the center of the rotation axis and the coordinates of the lower tangent point, the direction of the force vector formed by the center of the rotation axis and the lower tangent point is determined.

[0021] In some embodiments, the magnitude of the force vector is determined by the spring constant and the real-time deformation of the spring;

[0022] The real-time deformation is determined by the distance from the center of the rotation axis to the lower tangent point of the connecting line on the steering device, and the arc length between the lower tangent point of the connecting line and the lower tangent point on the steering device.

[0023] In some embodiments, the vector expression for the compensating torque on the joint axis is:

[0024]

[0025] In the formula, Represents a position vector; This represents a force vector.

[0026] In some embodiments, the method further includes:

[0027] The output torque of the joint motor is compensated according to the magnitude and direction of the compensating torque on the joint shaft; the joint motor is connected to the joint shaft.

[0028] Secondly, this embodiment provides a spring torque compensation device, including: a construction module, a first processing module, a second processing module, and a third processing module;

[0029] The construction module is used to construct a world coordinate system based on the kinematics of the dominant hand;

[0030] The first processing module is used to determine the position vector formed by the center of the joint axis to the center of the rotation axis based on the world coordinate system and the structural parameters of the main hand;

[0031] The second processing module is used to determine the force vector formed by the connection line from the center of the rotation axis to the lower tangent point on the steering device, based on the world coordinate system and the structural parameters of the main hand.

[0032] The third processing module determines the compensation torque on the joint axis based on the position vector and the force vector.

[0033] Thirdly, this embodiment provides an electronic device including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the steps of the spring torque compensation method described in the first aspect above.

[0034] Fourthly, this embodiment provides a storage medium storing a computer program that, when executed by a processor, implements the steps of the spring torque compensation method described in the first aspect above.

[0035] Compared with related technologies, the spring torque compensation method, device, electronic device, and storage medium provided in this embodiment establish a world coordinate system based on the kinematics of the master hand; determine the position vector formed by the center of the joint axis to the center of the rotation axis according to the world coordinate system and the structural parameters of the master hand; determine the force vector formed by the center of the rotation axis to the lower tangent point of the connecting line and the steering device according to the world coordinate system and the structural parameters of the master hand; and determine the compensation torque on the joint axis using a vector algorithm based on the position vector and the force vector. This solves the problem that it is necessary to manually judge the sign of the joint angle for subsequent calculations, and if the judgment is incorrect, it will lead to errors in the compensation torque. It achieves accurate calculation of the magnitude and direction of the compensation torque on the joint axis based on the position vector and the force vector, avoiding errors in torque compensation.

[0036] Details of one or more embodiments of this application are set forth in the following drawings and description to make other features, objects and advantages of this application more readily apparent. Attached Figure Description

[0037] The accompanying drawings, which are included to provide a further understanding of this application and form part of this application, illustrate exemplary embodiments and are used to explain this application, but do not constitute an undue limitation of this application. In the drawings:

[0038] Figure 1 This is a schematic diagram of an existing gravity compensation model;

[0039] Figure 2 This is a schematic diagram of the structure of a spring torque compensation system provided in an embodiment of this application;

[0040] Figure 3 This is a schematic diagram of a gravity compensation model provided in an embodiment of this application;

[0041] Figure 4 This is a schematic diagram of a gravity compensation model provided in another embodiment of this application;

[0042] Figure 5 This is a schematic diagram of a gravity compensation model provided in another embodiment of this application;

[0043] Figure 6 This is a flowchart of a spring torque compensation method provided in an embodiment of this application;

[0044] Figure 7 This is a schematic diagram illustrating the point of tangency between an external point of a circle and a circle, provided in an embodiment of this application.

[0045] Figure 8 This is a schematic diagram of vector rotation provided in an embodiment of this application;

[0046] Figure 9 This is a structural block diagram of a spring torque compensation device provided in an embodiment of this application.

[0047] In the diagram: 1. Fixed column; 2. Turntable; 3. Connecting rod; 4. Joint shaft; 5. Rotation shaft; 6. Steering device; 7. Spring; 8. Connecting line; 210. Building module; 220. First processing module; 230. Second processing module; 240. Third processing module. Detailed Implementation

[0048] To better understand the purpose, technical solution, and advantages of this application, the application is described and illustrated below in conjunction with the accompanying drawings and embodiments.

[0049] Unless otherwise defined, the technical or scientific terms used in this application shall have the general meaning as understood by one of ordinary skill in the art to which this application pertains. Words such as “a,” “an,” “an,” “the,” “the,” and “these,” used in this application, do not indicate quantitative limitation and may be singular or plural. The terms “comprising,” “including,” “having,” and any variations thereof used in this application are intended to cover non-exclusive inclusion; for example, a process, method, system, product, or device that comprises a series of steps or modules (units) is not limited to the listed steps or modules (units) but may include steps or modules (units) not listed, or may include other steps or modules (units) inherent to such processes, methods, products, or devices. The terms “connected,” “linked,” and “coupled,” used in this application, are not limited to physical or mechanical connections but may include electrical connections, whether direct or indirect. The term “multiple” used in this application refers to two or more. The "and / or" operator describes the relationship between related objects, indicating that three relationships can exist. For example, "A and / or B" can represent three cases: A alone, A and B simultaneously, and B alone. Typically, the character " / " indicates that the objects before and after it are in an "or" relationship. The terms "first," "second," and "third," etc., used in this application are merely for distinguishing similar objects and do not represent a specific ordering of the objects.

[0050] The design concept of this application is explained below:

[0051] Due to the inherent design characteristics of the master hand's mechanism, the output torque of the master hand's joint motor is often insufficient, requiring passive compensation methods. These include using counterweights or springs to offset some of the weight of the master hand's connecting rods, thereby improving the efficiency of the motor's output torque. However, since counterweight compensation significantly increases the overall weight and inertia of the master hand, spring compensation is often used in demanding applications. The advantage of spring compensation is that it does not increase the overall weight and inertia of the master hand. However, this method requires manual judgment of the joint angle for subsequent calculations; incorrect judgment can lead to errors in the compensation torque. Specifically, for example... Figure 1 As shown, the steering device 6 is considered a point mass. To calculate the torque of the spring force on the joint shaft 4, i.e., the spring's compensating torque, it is necessary to calculate the lever arm of the spring force on the joint shaft 4, as well as the spring's elongation. Calculating the spring's elongation requires first determining the length of PR, and then indirectly determining the spring's elongation. The length of PR is affected by the joint angle ∠POR; the sign of this joint angle needs to be determined manually. An incorrect determination will lead to an incorrect direction of the compensating torque.

[0052] In view of this, this application designs a spring torque compensation method, device, electronic device, and storage medium. The method, based on the structural parameters related to the spring torque compensation system and a world coordinate system established based on the kinematics of the master hand, determines the position vector and force vector used to calculate the spring compensation torque; finally, based on the position vector and force vector, it determines the compensation torque on the joint axis; this achieves accurate calculation of the magnitude and direction of the compensation torque on the joint axis, avoiding torque compensation errors; and solves the problem that requires manual judgment of the sign of the joint angle for subsequent calculations, which, if incorrect, leads to errors in the compensation torque.

[0053] To better understand the design concept of this application, the following are application scenarios of the spring torque compensation method in a spring torque compensation system.

[0054] Please refer to Figure 2 This paper presents an example diagram illustrating an application scenario of a spring torque compensation method in a spring torque compensation system. In this scenario, the spring torque compensation system includes a fixed post 1, a turntable 2, a connecting rod 3, a joint shaft 4, a rotating shaft 5, a steering device 6, a spring 7, and a connecting line 8. The fixed post 1 is positioned at any point above and outside the turntable 2; the steering device 6 is positioned at any point below the turntable 2; one end of the spring 7 is connected to the fixed post 1, and the other end of the spring 7 is connected to one end of the connecting line 8; the other end of the connecting line 8 is connected to the rotating shaft 5 via the steering device 6; the connecting rod 3 is mounted on the turntable 2 with the joint shaft 4 as a fixed point; and the rotating shaft 5 rotates along the turntable 2.

[0055] The fixed column 1, joint shaft 4, rotating shaft 5, and steering device 6 can all be made of metal and can be considered rigid components that will not deform during use. The steering device 6 can be a pulley, steering wheel, etc. The turntable 2 and connecting rod 3 can be made of metal or other materials. The turntable 2 is fixed, and the user can move the rotating shaft 5 along the outer circumference of the turntable 2 by holding the connecting rod 3 under the user's force. In this embodiment, the spring 7 can be a helical spring, which is in a stretched state. The connecting wire 8 can be made of metal and can be considered a rigid component. Specifically, the connecting wire 8 can be a steel wire rope, etc. One end of the connecting wire 8 is connected to the other end of the spring 7, and the other end of the connecting wire 8 is connected to the rotating shaft 5 via the steering device 6.

[0056] The positions of the fixed column 1, the joint shaft 4, and the steering device 6 are fixed relative to the turntable 2. For example, the fixed column 1 is located at any point above the turntable 2; the steering device 6 is located at any point below the turntable 2; the joint shaft 4 is located on the turntable 2 between the fixed column 1 and the steering device 6, and the joint shaft 4 is located at the center of the turntable 2; preferably, the center of the joint shaft 4 coincides with the center of the turntable 2. A motor is installed on the connecting rod 3 on the side of the rotating shaft 5. Under the action of the force generated by the motor, the weight of the connecting rod 3 is counteracted by the joint shaft 4 as the balance point, achieving dynamic balance. Figure 3 , Figure 4 as well as Figure 5 The figure shows a schematic diagram of the gravity compensation model of the rotating shaft 5 at different positions on the turntable 2 (the connecting rod 3 is omitted in the figure, and the spring is represented by a straight line).

[0057] When calculating the spring torque compensation on joint axis 4, a processor can be set up in the spring torque compensation system to handle the process. The specific processing steps can be as follows: establish a world coordinate system based on the kinematics of the master hand; determine the position vector formed by the center of joint axis 4 to the center of rotation axis 5 according to the world coordinate system and the structural parameters of the master hand; determine the force vector formed by the center of rotation axis 5 to the point of tangency between the connecting line 8 and the steering device 6 according to the world coordinate system and the structural parameters of the master hand; determine the magnitude and direction of the compensation torque on joint axis 4 based on the position vector and the force vector. Those skilled in the art will understand that the above... Figure 2 The structure shown is for illustrative purposes only and does not limit the structure of the aforementioned spring torque compensation system. For example, the spring torque compensation system may also include a... Figure 2 The more or fewer components shown, or having the same Figure 2 The different configurations shown are illustrated. For example, the aforementioned spring torque compensation system may also include a memory and a transmission device.

[0058] The memory can be used to store computer programs, such as application software programs and modules, like the computer program corresponding to the spring torque compensation method in this embodiment. The processor executes various functional applications and data processing by running the computer program stored in the memory, thereby implementing the above-described method. The memory may include high-speed random access memory (RAM) and non-volatile memory, such as one or more magnetic storage devices, flash memory, or other non-volatile solid-state memory. In some instances, the memory may further include memory remotely located relative to the processor, and these remote memories can be connected to the platform via a network. Examples of such networks include, but are not limited to, the Internet, corporate intranets, local area networks (LANs), mobile communication networks, and combinations thereof.

[0059] The transmission device is used to receive or send data via a network. This network includes wireless networks provided by the platform's communication provider. In one example, the transmission device includes a Network Interface Controller (NIC), which can connect to other network devices via a base station to communicate with the Internet. In another example, the transmission device may be a Radio Frequency (RF) module, used for wireless communication with the Internet.

[0060] In other embodiments, the mechanical relationships between the fixed post 1, spring 7, steering device 6, connecting line 8, turntable 2, and rotating shaft 5 remain unchanged. That is, the fixed post 1 is located at any point above the turntable 2; the steering device 6 is located at any point below the turntable 2; one end of the spring 7 is connected to the fixed post 1, and the other end of the spring 7 is connected to one end of the connecting line 8; the other end of the connecting line 8 is connected to the rotating shaft 5 via the steering device 6; the connecting rod 3 is fixed to the turntable 2 with the joint shaft 4 as the fixed point, and the joint shaft 4 is located at the center of the turntable 2; the rotating shaft 5 rotates along the turntable 2, and these properties remain unchanged. The specific structure of each component is not limited.

[0061] based on Figure 2 The application scenario shown in this embodiment provides a spring torque compensation method. Figure 6 This is a flowchart of the spring torque compensation method in this embodiment, as follows: Figure 6 As shown, the process includes the following steps:

[0062] Step S210: Establish a world coordinate system based on the kinematics of the dominant hand;

[0063] Step S220: Based on the world coordinate system and the structural parameters of the master hand, determine the position vector formed by the center of the joint axis and the center of the rotation axis;

[0064] Step S230: Based on the world coordinate system and the structural parameters of the master, determine the force vector formed by the connection line from the center of the rotation axis to the point of tangency on the steering device;

[0065] Step S240: Determine the compensating torque on the joint axis based on the position vector and force vector.

[0066] It should be noted that the master hand is the carrier through which the user interacts with the minimally invasive surgical robot. Master hand kinematics refers to the branch of mechanics that describes and studies the changes in the position of relevant components in the master hand over time from a geometric perspective (referring to the physical properties of the object itself and the forces applied to the object).

[0067] Establishing a world coordinate system based on the kinematics of the main hand can be done by basing it on the positional relationships of related components in a fixed position within the main hand. For example: A world coordinate system can be established with the center of a joint axis, steering pulley, or fixed column, using a horizontal line passing through that center as the X-axis and a vertical line passing through that center as the Y-axis. Another example: A world coordinate system can be established with the intersection of a joint axis and a turntable in the main hand as the center, using a straight line passing through the center of the joint axis and the center of the rotation axis as the X-axis, the direction from the center of the joint axis to the center of the rotation axis as the X-axis, and a straight line passing through the center of the joint axis and perpendicular to the X-axis as the Y-axis, with the direction from the center of the joint axis to the rotation axis as the Y-axis. Yet another example: A world coordinate system can be established with the center of a fixed column in the main hand as the center of the world coordinate system, using a straight line passing through this center and horizontal to the line connecting the centers of the joint axis and the rotation axis as the X-axis, and a straight line passing through this center and perpendicular to the X-axis as the Y-axis. Other methods for establishing a world coordinate system based on the kinematics of the dominant hand will not be listed here. However, it should be noted that world coordinate systems established by different dominant hand kinematics can be interconnected based on the structural parameters of the dominant hand. Preferably, aligning the center of the joint axis with the center of the turntable, and then establishing a world coordinate system based on the center of the joint axis, can further simplify the calculation process and improve computational efficiency.

[0068] The structural parameters of the main arm refer to the structural parameters of each component in the main arm. These include the length, radius, elastic coefficient, relative positional relationship, and relative angle of components such as the fixed column, turntable, connecting rod, joint axis, rotation axis, steering mechanism, spring, and connecting line. These parameters are not listed individually here. Using these structural parameters, the positions of each component can be transformed into a world coordinate system, obtaining their corresponding coordinates and angular relationships. This allows the determination of the position vector formed by the center of the joint axis to the center of the rotation axis, and the force vector formed by the center of the rotation axis to the point of tangency between the connecting line and the steering mechanism. For example, relative to a coordinate system established with the intersection of the joint axis and the turntable in the main arm as the center, the coordinates of the joint axis are (0, 0). Of course, the specific coordinates of the joint axis will differ for different joint axes.

[0069] Once the position vector and force vector are determined, the magnitude and direction of the compensating torque on the joint axis can be calculated. For example, the coordinates of the center P of the rotation axis can be directly calculated as follows: P(x p ,y p )=f(θ)=(h s sinθ,h s Therefore, based on the rotation angle and length of OP in the world coordinate system and the structural parameters of the master hand, the coordinates of the center point P of the rotation axis can be determined. Then, a vector algorithm is used to calculate the magnitude and direction of the compensation torque on the joint axis. The change in the angle θ between the position vector and the X-axis centered on the joint axis directly relates to the direction of the compensation torque on the joint axis, thus achieving accurate calculation of the magnitude and direction of the compensation torque and avoiding errors in torque compensation.

[0070] Through the above steps, a world coordinate system is first established based on the kinematics of the master hand. Then, combined with the structural parameters of the master hand, the position vector formed by the center of the joint axis to the center of the rotation axis is determined; and the force vector formed by the center of the rotation axis to the point of tangency between the connecting line and the steering device is determined. Finally, based on the position vector and the force vector, the magnitude and direction of the compensation torque on the joint axis can be directly calculated, avoiding errors in torque compensation. This solves the problem in related technologies where it is necessary to manually determine the sign of the joint angle before subsequent calculations, and if the determination is incorrect, it will lead to errors in the compensation torque.

[0071] In some embodiments, step S210 includes the following steps:

[0072] Step S211: Establish the world coordinate system with the intersection of the joint axis and the turntable in the main hand as the center, the straight line passing through the center of the joint axis circle and the center of the rotation axis circle as the X-axis, the direction from the center of the joint axis circle to the center of the rotation axis circle as the direction of the X-axis, the straight line passing through the center of the joint axis circle and perpendicular to the X-axis as the Y-axis, and the direction from the center of the joint axis circle to the rotation axis as the direction of the Y-axis.

[0073] Specifically, such as Figures 3 to 5 The world coordinate system shown has the center point O as (x o ,y o The center of the circle is (0,0). The X and Y axes pass through the center of the circle; the coordinates of point P are (x...). p ,y p Position vector The sign of the rotation angle θ along the X-axis and the position of point P are related. For example, from... Figure 3 The position of point P changes to Figure 2 The position of point P in the middle is such that the rotation angle θ is positive. From... Figure 3 The position of point P changes to Figure 4 The position of point P is such that the rotation angle θ is negative. Since the coordinates of the center P of the rotation axis can be calculated from the rotation angle θ, and are not affected by the zero position or positive / negative value of the rotation angle θ, the calculation only requires solving the coordinates of each key coordinate point in the world coordinate system. Then, the magnitude and direction of the compensation torque are calculated based on the vector algorithm, thereby improving the accuracy of the compensation torque direction judgment and reducing the problem of errors in compensation torque that easily occur in related technologies. Moreover, the world coordinate system provided in this embodiment, relative to the world coordinate system established by the structural parameters of other components, can simplify the conversion relationship between various vectors and parameters, and improve the calculation efficiency.

[0074] In other embodiments, a world coordinate system can also be established based on the structural parameters of other components, based on a similar establishment process, which will not be elaborated here.

[0075] In some embodiments, step S220 includes the following steps:

[0076] Step S221: Determine the coordinates of the joint axis center and the rotation axis center based on the world coordinate system and the structural parameters of the master hand;

[0077] Step S222: Determine the position vector formed by the joint axis center and the rotation axis center based on the coordinates of the joint axis center and the rotation axis center.

[0078] Specifically, the structural parameters of the master arm refer to the structural parameters of each component within the master arm. Based on the world coordinate system, the mechanical relationships between the components of the master arm are transformed into the world coordinate system, thereby determining the coordinates of the joint axis centers and the rotation axis centers. The position vector is then obtained by pointing from the joint axis center to the rotation axis center. Because position vectors are special—while general vectors are only related to coordinate representation and the choice of coordinate system—the magnitude and direction of position vectors are also related to the choice of coordinate system (the choice of origin). In this embodiment, a world coordinate system with the joint axis center as the origin is preferred. If other coordinate systems are used, they need to be transformed to have the joint axis center as the origin before calculation. Figure 3 In the middle, the coordinates of the center of the joint axis are (x... o ,y o ); the coordinates of the center of the rotation axis are (x p ,y p ).

[0079] In some embodiments, step S230 includes the following steps:

[0080] Step S231: Determine the coordinates of the center of the steering device based on the world coordinate system and the structural parameters of the master arm;

[0081] Step S232: Using the formula for the external tangent point of a circle, determine the coordinates of the lower tangent point of the connecting line and the steering device based on the coordinates of the center of the rotating shaft, the center of the steering device, and the radius.

[0082] Step S233: Determine the direction of the force vector formed by the center of the rotation axis and the lower tangent point based on the coordinates of the center of the rotation axis and the lower tangent point.

[0083] Specifically, the structural parameters of the main steering mechanism refer to the structural parameters of each component within it. Based on the world coordinate system, the mechanical relationships between the components are transformed into this system, allowing the determination of the coordinates of the steering device's center and the rotation axis's center. Combining the steering device's radius with the external tangent formula, the coordinates of the lower tangent point on the connecting line and the steering device can be determined. The lower tangent point is the point of tangency on the connecting line near the rotation axis's center, while the upper tangent point is the point of tangency on the connecting line near the joint axis's center. Therefore, the direction of the force vector formed by the rotation axis's center pointing towards the lower tangent point is determined.

[0084] like Figure 7 The diagram shown illustrates the principle of finding the point of tangency between an external point and a circle. The formula for finding the point of tangency outside a circle is expressed as follows:

[0085]

[0086] In the formula, (x q ,y qLet (x) be the X-axis and Y-axis coordinates of the inverted point Q; p ,y p Let ) represent the X-axis and Y-axis coordinates of the center point P of the rotation axis; PQ Let β be the length between the center point P of the rotation axis and the point of tangency Q. Let β be the distance between the x-axis and the x-axis in the world coordinate system. The angle between them.

[0087] Using the formula for the external tangent point of a circle, combined with the coordinates of the center of the rotating shaft, the center of the steering device, and its radius, the coordinates of the lower tangent point of the connecting line on the steering device can be determined. Then, the force vector can be obtained from the direction of the rotating shaft center pointing to the lower tangent point.

[0088] The magnitude of the force vector is determined by the spring constant and the real-time deformation.

[0089] The real-time deformation is determined by the distance from the center of the rotating shaft to the lower tangent point of the connecting line on the steering device, and the arc length between the lower tangent point and the lower tangent point on the steering device.

[0090] The expression for the magnitude of the force vector is:

[0091] In the formula, K represents the magnitude of the force vector. s ΔL is the spring constant; s This represents the real-time deformation of the spring.

[0092] The expression for the real-time deformation of the spring is:

[0093] In the formula, ΔL s0 It is the initial deformation of the spring when it is in the initial zero position, l s0 PQ and arc length at initial zero position The length of l PQ It is the length of PQ; It is the arc length of QR.

[0094] The above formula can be used to accurately calculate the force vector.

[0095] exist Figure 1 In existing solutions, the steering device 6 is simplified to a point mass. This calculation method leads to errors in the actual calculation of the compensation torque, including: deviations in the actual direction of the spring tension, errors in the lever arm of the spring tension relative to the joint shaft 4, and errors in the spring elongation caused by changes in the wrap angle of the connecting line around the steering device 6. In this embodiment, a vector method is used to accurately calculate the actual direction of the spring tension, changes in the wrap angle of the steering device 6, etc., thereby improving the accuracy of the compensation torque calculation.

[0096] like Figure 8As shown, after a vector is rotated by an angle α, the coordinates of its endpoint can be calculated using the following formula:

[0097] x1=|R|cos(δ+α)=|R|cosδcosα+|R|sinδsinα=x0cosα-y0sinα;

[0098] y1=|R|sin(δ+α)=|R|sinδcosα-|R|cosδsinα=y0cosα+x0sinα;

[0099] In the formula, (x0, y0) is a vector. The coordinates; (x1, y1) is a vector. Coordinates; δ is the angle between the X-axis and the original position of the vector; vector It is a vector Obtained after rotating by an angle α.

[0100] based on Figure 8 The angle α between two vectors can be calculated using the following formula:

[0101]

[0102] In some embodiments, the vector expression for the compensating torque on the joint axis is:

[0103]

[0104] In the formula, Represents a position vector; This represents a force vector.

[0105] Specifically, the compensating torque on the joint axis is the result of the cross product of two position vectors. The magnitude of the compensating torque is determined by the two vectors and the angle α between them. The expression for the magnitude of the compensating torque is: Among them, the position vector The magnitude of the force vector is determined by the distance from the center of the joint axis to the center of the rotation axis. The direction of the compensating torque is determined by the spring constant and the real-time deformation. It can be calculated from the included angle or determined by the right-hand rule. The magnitude and direction of the compensating torque on the joint axis can be accurately calculated using the above formula.

[0106] In some embodiments, the spring torque compensation method further includes the following steps:

[0107] The output torque of the joint motor is compensated according to the magnitude and direction of the compensating torque on the joint shaft; the joint motor is connected to the joint shaft.

[0108] Specifically, the joint motor is connected to the joint shaft, which drives the rotating shaft to rotate along the turntable. Due to the characteristics of the main hand mechanism design, the output torque of the joint motor is often insufficient. Compensation is achieved by adjusting the magnitude and direction of the compensating torque on the joint shaft, thereby improving the compensation accuracy and ensuring that the compensation direction is correct. In other embodiments, the joint motor can also be connected to a connecting rod; this is not a limitation.

[0109] It should be noted that the steps shown in the above process or in the flowcharts of the accompanying figures can be executed in a computer system such as a set of computer-executable instructions. Furthermore, although a logical order is shown in the flowcharts, in some cases the steps shown or described may be performed in a different order than that shown here. For example, steps S220 and S230 can be interchanged.

[0110] This embodiment also provides a spring torque compensation device, which is used to implement the above embodiments and preferred embodiments; details already described will not be repeated. The terms "module," "unit," "subunit," etc., used below refer to combinations of software and / or hardware that implement a predetermined function. Although the device described in the following embodiments is preferably implemented in software, hardware implementation, or a combination of software and hardware, is also possible and contemplated.

[0111] Figure 9 This is a structural block diagram of the spring torque compensation device in this embodiment, as shown below. Figure 9 As shown, the device includes: a construction module 210, a first processing module 220, a second processing module 230, and a third processing module 240;

[0112] Module 210 is used to construct a world coordinate system based on the kinematics of the dominant hand;

[0113] The first processing module 220 is used to determine the position vector formed by the center of the joint axis to the center of the rotation axis according to the world coordinate system and the structural parameters of the main hand;

[0114] The second processing module 230 is used to determine the force vector formed by the connection line from the center of the rotation axis to the lower tangent point on the steering device, based on the world coordinate system and the structural parameters of the main hand.

[0115] The third processing module 240 determines the compensation torque on the joint axis based on the position vector and the force vector.

[0116] The above-mentioned device can directly correlate and calculate the magnitude and direction of the compensation torque on the joint axis, avoiding errors in torque compensation; it solves the problem in related technologies that require manual judgment of the sign of the joint angle before subsequent calculation, which can lead to errors in the compensation torque if the judgment is incorrect.

[0117] In some embodiments, the construction module 210 is further configured to establish the world coordinate system with the intersection of the joint axis and the turntable in the master hand as the center, the straight line passing through the center of the joint axis and the center of the rotation axis as the X-axis, the direction from the center of the joint axis to the center of the rotation axis as the direction of the X-axis, the straight line passing through the center of the joint axis and perpendicular to the X-axis as the Y-axis, and the direction from the center of the joint axis to the rotation axis as the direction of the Y-axis.

[0118] In some of these embodiments, the center of the joint axis coincides with the center of the turntable.

[0119] In some embodiments, the first processing module 220 is further configured to determine the coordinates of the joint axis center and the rotation axis center based on the world coordinate system and the structural parameters of the master hand;

[0120] Based on the coordinates of the joint axis center and the rotation axis center, determine the position vector formed by the joint axis center and the rotation axis center.

[0121] In some embodiments, the second processing module 230 is also configured to determine the coordinates of the center of the steering device based on the world coordinate system and the structural parameters of the master hand.

[0122] Using the formula for the external tangent point of a circle, the coordinates of the lower tangent point of the connecting line and the steering device are determined based on the coordinates of the center of the rotating shaft, the center of the steering device, and the radius.

[0123] Determine the direction of the force vector formed by the center of the rotation axis and the lower tangent point based on the coordinates of the center of the rotation axis and the lower tangent point.

[0124] In some of these embodiments, the magnitude of the force vector is determined by the spring constant and the real-time deformation;

[0125] The real-time deformation is determined by the distance from the center of the rotating shaft to the lower tangent point of the connecting line on the steering device, and the arc length between the lower tangent point and the lower tangent point on the steering device.

[0126] In some embodiments, the vector expression for the compensating torque on the joint axis is:

[0127]

[0128] In the formula, Represents a position vector; This represents a force vector.

[0129] In some of these embodiments, Figure 9 Based on this, the spring torque compensation system also includes a compensation module;

[0130] The compensation module is used to compensate the output torque of the joint motor according to the magnitude and direction of the compensation torque on the joint shaft; the joint motor is connected to the joint shaft.

[0131] It should be noted that the above modules can be functional modules or program modules, and can be implemented through software or hardware. For modules implemented through hardware, the above modules can reside in the same processor; or the above modules can be located in different processors in any combination.

[0132] This embodiment also provides an electronic device including a memory and a processor, the memory storing a computer program and the processor being configured to run the computer program to perform the steps in any of the above method embodiments.

[0133] Optionally, the electronic device may further include a transmission device and an input / output device, wherein the transmission device is connected to the processor and the input / output device is connected to the processor.

[0134] Optionally, in this embodiment, the processor can be configured to perform the following steps via a computer program:

[0135] S1, establishing a world coordinate system based on the kinematics of the dominant hand;

[0136] S2, based on the world coordinate system and the structural parameters of the master hand, determine the position vector formed by the center of the joint axis and the center of the rotation axis;

[0137] S3, based on the world coordinate system and the structural parameters of the master, determine the force vector formed by the line from the center of the rotation axis to the point of tangency between the connecting line and the steering device;

[0138] S4. Determine the compensating torque on the joint axis based on the position vector and force vector.

[0139] It should be noted that the specific examples in this embodiment can refer to the examples described in the above embodiments and optional implementations, and will not be repeated in this embodiment.

[0140] Furthermore, in conjunction with the spring torque compensation method provided in the above embodiments, this embodiment can also provide a storage medium for implementation. The storage medium stores a computer program; when executed by a processor, the computer program implements any of the spring torque compensation methods described in the above embodiments.

[0141] It should be understood that the specific embodiments described herein are merely illustrative of the application and not intended to limit it. All other embodiments derived by those skilled in the art based on the embodiments provided in this application without inventive effort are within the scope of protection of this application.

[0142] Obviously, the accompanying drawings are merely some examples or embodiments of this application. Those skilled in the art can apply this application to other similar situations based on these drawings without any creative effort. Furthermore, it is understood that although the work done in this development process may be complex and lengthy, for those skilled in the art, certain design, manufacturing, or production modifications made based on the technical content disclosed in this application are merely conventional technical means and should not be considered as insufficient disclosure of this application.

[0143] The term "embodiment" in this application refers to a specific feature, structure, or characteristic described in connection with an embodiment that may be included in at least one embodiment of this application. The appearance of this phrase in various places in the specification does not necessarily imply the same embodiment, nor does it imply that it is mutually exclusive with or independent of other embodiments. It will be clearly or implicitly understood by those skilled in the art that the embodiments described in this application may be combined with other embodiments without conflict.

[0144] The embodiments described above are merely illustrative of several implementation methods of this application, and while the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of patent protection. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of this application, and these all fall within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the appended claims.

Claims

1. A spring torque compensation method, characterized in that, This invention relates to a spring torque compensation system, which includes a fixed post, a turntable, a connecting rod, a joint shaft, a rotating shaft, a steering device, a spring, and a connecting line. The fixed post is located at any point above and outside the turntable. The steering device is located at any point below the turntable. One end of the spring is connected to the fixed post, and the other end of the spring is connected to one end of the connecting line. The other end of the connecting line is connected to the rotating shaft via the steering device. The connecting rod is mounted on the turntable with the joint axis as a fixed point. The rotating shaft rotates along the turntable; the method includes: Establish a world coordinate system based on the kinematics of the dominant hand; Based on the world coordinate system and the structural parameters of the main hand, determine the position vector formed by the center of the joint axis and the center of the rotation axis; Based on the world coordinate system and the structural parameters of the main hand, determine the force vector formed by the line from the center of the rotation axis to the point of tangency on the steering device. The compensating torque on the joint axis is determined based on the position vector and the force vector.

2. The spring torque compensation method according to claim 1, characterized in that, The establishment of a world coordinate system based on the kinematics of the dominant hand includes: A world coordinate system is established with the intersection of the joint axis and the turntable in the main hand as the center, the straight line passing through the center of the joint axis and the center of the rotation axis as the X-axis, the direction from the center of the joint axis to the center of the rotation axis as the direction of the X-axis, the straight line passing through the center of the joint axis and perpendicular to the X-axis as the Y-axis, and the direction from the center of the joint axis to the rotation axis as the direction of the Y-axis.

3. The spring torque compensation method according to claim 1, characterized in that, The center of the joint axis coincides with the center of the turntable.

4. The spring torque compensation method according to any one of claims 1 to 3, characterized in that, The step of determining the position vector formed by the center of the joint axis and the center of the rotation axis based on the world coordinate system and the structural parameters of the main hand includes: Based on the world coordinate system and the structural parameters of the main hand, determine the coordinates of the center of the joint axis and the center of the rotation axis; Based on the coordinates of the center of the joint axis and the center of the rotation axis, determine the position vector formed by the distance from the center of the joint axis to the center of the rotation axis.

5. The spring torque compensation method according to any one of claims 1 to 3, characterized in that, The step of determining the force vector formed by the line from the center of the rotation axis to the point of tangency on the steering device, based on the world coordinate system and the structural parameters of the main hand, includes: Based on the world coordinate system and the structural parameters of the master hand, determine the coordinates of the center of the steering device. Using the formula for the external tangent point of a circle, the coordinates of the lower tangent point of the connecting line and the steering device are determined based on the coordinates of the center of the rotating shaft, the coordinates of the center of the steering device, and the radius. Based on the coordinates of the center of the rotation axis and the coordinates of the lower tangent point, the direction of the force vector formed by the center of the rotation axis and the lower tangent point is determined.

6. The spring torque compensation method according to claim 5, characterized in that, The magnitude of the force vector is determined by the spring constant and the real-time deformation of the spring; The real-time deformation is determined by the distance from the center of the rotation axis to the lower tangent point of the connecting line on the steering device, and the arc length between the lower tangent point of the connecting line and the lower tangent point on the steering device.

7. The spring torque compensation method according to claim 1, characterized in that, The vector expression for the compensating torque on the joint axis is: ； In the formula, Represents a position vector; This represents a force vector.

8. The spring torque compensation method according to claim 1, characterized in that, The method further includes: The output torque of the joint motor is compensated according to the magnitude and direction of the compensating torque on the joint shaft; the joint motor is connected to the joint shaft.

9. A spring torque compensation device, characterized in that, This invention relates to a spring torque compensation system, which includes a fixed post, a turntable, a connecting rod, a joint shaft, a rotating shaft, a steering device, a spring, and a connecting line. The fixed post is located at any point above and outside the turntable. The steering device is located at any point below the turntable. One end of the spring is connected to the fixed post, and the other end of the spring is connected to one end of the connecting line. The other end of the connecting line is connected to the rotating shaft via the steering device. The connecting rod is mounted on the turntable with the joint axis as a fixed point. The rotating shaft rotates along the turntable; it includes: a construction module, a first processing module, a second processing module, and a third processing module; The construction module is used to construct a world coordinate system based on the kinematics of the dominant hand; The first processing module is used to determine the position vector formed by the center of the joint axis to the center of the rotation axis based on the world coordinate system and the structural parameters of the main hand; The second processing module is used to determine the force vector formed by the connection line from the center of the rotation axis to the lower tangent point on the steering device, based on the world coordinate system and the structural parameters of the main hand. The third processing module determines the compensation torque on the joint axis based on the position vector and the force vector.

10. An electronic device comprising a memory and a processor, characterized in that, The memory stores a computer program, and the processor is configured to run the computer program to perform the steps of the spring torque compensation method according to any one of claims 1 to 8.

11. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the steps of the spring torque compensation method according to any one of claims 1 to 8.

Images