A Motion Control Method Based on a Twisted Wire Drive System and Its Application

Abstract

The present invention discloses a motion control method and its application based on a twisted wire drive system. The method includes: 1. Establishing a five-dimensional projective space based on the twisted wire drive mechanism and describing the Plücker coordinates of a general straight line in the projective space; 2. Conducting kinematic decomposition on the twisted wire drive system, decomposing the twisted wire motion into rotational motion around an axis and translational motion in the axial direction, and calculating the velocity screw and force screw of the system; 3. Calculating the finite displacement screw matrix of the system to achieve position control of the twisted wire drive system. Aiming at flexible drive technology, the present invention adopts a new kinematic calculation method. According to the given spatial conditions of the system and the angular velocity provided by the motor, it uses a six-coordinate method to accurately describe the kinematics of the drive system, and accurately controls the drive speed, force, and position parameters, thereby reducing the complexity of system control and improving the accuracy and stability of control.

Description

Technical Field

[0001] The present invention belongs to the field of motion control of flexible drive technology, and specifically relates to a motion control method based on a twisted wire drive system and its application. Background Art

[0002] Traditional robot drives mainly rely on motors and require rigid and complex reduction transmission mechanisms. The drives are large in volume, and their weight and complexity are relatively high. The twisted wire drive system can achieve high-performance linear transmission with a relatively simple system structure. It can generate a linear motion with high tensile force from the relatively low torque obtained by a rotating motor, thus meeting the wide requirements of highly integrated electromechanical devices for output speed and force. Generally speaking, by establishing a system motion control equation, relatively accurate motion analysis and control of the drive process of the drive system can be achieved. However, the motion process of the twisted wire drive system has obvious non-linearity. According to traditional kinematic analysis methods, the calculation process of its motion control equation is cumbersome and has large errors, and it is not easy to establish a relatively accurate motion equation for the twisted wire drive system. In addition, the traditional kinematic analysis method of the twisted wire drive system does not describe the system motion process intuitively enough, the relationship between motion parameters is not clear enough, the control of the motion process is not accurate enough, and the control stability is poor, thus bringing great obstacles to the research of the twisted wire drive system and restricting the development of the twisted wire drive system. Summary of the Invention

[0003] The present invention is to solve the deficiencies of the above-mentioned existing technologies, and proposes a motion control method based on a twisted wire drive system and its application, in order to achieve precise control of drive speed, force, and position parameters, thereby reducing the complexity of system control, improving the accuracy and stability of control, and solving the problems of large control errors and unstable control in the existing technologies.

[0004] The present invention adopts the following technical solutions to achieve the above invention purposes:

[0005] The motion control method based on a twisted wire drive system of the present invention is characterized in that it is carried out according to the following steps:

[0006] Step 1: Construct a five-dimensional projective space coordinate system P of the twisted wire drive system 5 :

[0007] Taking the end of the rotation center axis of the twisted wire drive system as the origin O, taking the direction parallel to the rotation center axis as the Z-axis, taking a straight line perpendicular to the Z-axis and passing through the origin O as the X-axis, and taking a straight line perpendicular to the X-axis and Z-axis and passing through the origin O as the Y-axis, establish a global space coordinate system O-XYZ;

[0008] Taking an arbitrary position point on the torsion wire in the torsion wire drive system as the origin P, taking the straight line parallel to the Z-axis and passing through the origin P as the z-axis, taking the straight line parallel to the X-axis and passing through the origin P as the x-axis, and taking the straight line perpendicular to the x-axis and z-axis and passing through the origin P as the y-axis, a projective space coordinate system P-xyz is established;

[0009] The five-dimensional projective space coordinate system P is composed of the global space coordinate system O-XYZ and the projective space coordinate system P-xyz 5 ;

[0010] Step 2: Establish the Plücker coordinates of the torsion wire at the origin P of the projective space coordinate system P-xyz;

[0011] Step 2.1: Assume that the torsion wire is at the point P' above the origin P. Denote the coordinates of the origin P and the point P' in the global coordinate system O-XYZ as P(x1, y1, z1) and P'(x2, y2, z2) respectively, and use the attitude vector between the origin P and the point P' as the direction of the torsion wire motion vector, and use Equation (1) to obtain the attitude vector of the torque on the origin O of the global space coordinate system O-XYZ

[0012]

[0013] In Equation (1), p, q, and r respectively represent the components of the attitude vector of the torque taken on the origin O of the global coordinate system O-XYZ in the X-axis, Y-axis, and Z-axis directions, and p = y1z2 - y2z1, q = x2z1 - x1z2, r = x1y2 - x2y1;

[0014] Step 2.2: In the global space coordinate system O-XYZ, use Equation (2) to obtain the Plücker coordinates of the torsion wire

[0015]

[0016] Step 3: Use Equation (3) and Equation (4) to obtain the descriptions of the angular velocity and linear velocity of the torsion wire in the global space coordinate system O-XYZ, and thus obtain the velocity screw matrix T of the torsion wire in the torsion wire drive system from Equation (5):

[0017]

[0018]

[0019]

[0020] In Equations (3) and (4), α represents the helix angle of the twisted wire in the twisted wire drive system, and β represents the angle between the projection of the line segment PP′ between the origin P and point P′ on the projection plane P-xy and the x-axis direction; ω x , ω y , ω z respectively represent the components of the resultant angular velocity ω′ of the twisted wire on the x-axis, y-axis, and z-axis, ν x , ν y , ν z respectively represent the components of the resultant velocity ν′ of the twisted wire on the x-axis, y-axis, and z-axis;

[0021] In Equation (5), ω represents the angular velocity of the twisted wire in the projection plane P-xy, and its vector direction points from the origin P to the projection point of P′ on the projection plane, and the scalar magnitude ||ω|| is equal to the motor torsional angular velocity ω t ; ω′ and ν′ respectively represent the resultant angular velocity and resultant velocity of the twisted wire, and their vector directions are both the same as direction; ν represents the velocity of the twisted wire in the projection plane P-xy, and its vector direction is the same as ω;

[0022] Step 4: Obtain the force screw matrix W of the twisted wire in the twisted wire drive system:

[0023] Step 4.1: Use Equation (6) to obtain the resultant force vector of the twisted wire in the twisted wire drive system

[0024]

[0025] In Equation (6), ||f′|| represents the scalar magnitude of the resultant force on the twisted wire in the twisted wire drive system, and ||f′|| = ||f|| / cosα, ||f|| represents the scalar of the force on the projection plane of the twisted wire, and is calculated from the motor angular velocity ω t and the rotation radius r of the twisted wire drive system;

[0026] Step 4.2: Use Equations (8) and (9) to obtain the two components after decomposing the resultant couple vector :

[0027]

[0028]

[0029] In Equations (8) and (9), represents the component in the same direction as , represents the component orthogonal to , c represents the projection of the resultant couple vector on direction, A vector indicating the intersection point of the projective plane P-xy and the central rotation axis of the torsion wire drive system, pointing from the intersection point to the origin P of the projective space coordinate system P-xyz;

[0030] Step 4.3: Obtain the force screw matrix W of the torsion wire in the torsion wire drive system by using Equation (10):

[0031]

[0032] In Equation (10), denotes the resultant force vector acting on the torsion wire, denotes the resultant couple vector of the torsion wire with respect to the origin P of the projective space coordinate system P-xyz;

[0033] Step 5: Obtain the rotation matrix R of the torsion wire drive system by using Equation (11):

[0034] R = I + sinθ·A s +(1 - cosθ)·A s ·A s (11)

[0035] In Equation (11), I is the identity matrix, and A s is the skew-symmetric matrix of, where is the central axis vector of the rotation axis, and s x 、s y 、s z respectively represent the components of the rotation central axis in the x-axis, y-axis, and z-axis directions; θ is the motor rotation angle;

[0036] Step 6: According to the translation vector of the torsion wire drive system, obtain the matrix A for realizing the position change of the screw axis by using Equation (12):

[0037]

[0038] In Equation (12), d x 、d y 、d z respectively represent the components of the translation vector of the torsion wire drive system in the x-axis, y-axis, and z-axis directions;

[0039] Step 7: Use Equation (13) and Equation (14) to respectively obtain the final pose of the torsion wire in the torsion wire drive system after rotation and translation movements

[0040]

[0041]

[0042] In equations (13) and (14), represents the Plücker coordinates of the twisted line, i.e., the initial pose vector of the twisted line; represents the final pose of the twisted line after rotation and translation; tr represents the trace of the matrix;

[0043] Step 8: Use equation (14) to obtain the finite displacement screw matrix Y of the twisted line in the twisted line drive system:

[0044]

[0045] In equation (14), represents the vector pointing from the origin O of the global space coordinate system O-XYZ to the origin P of the projective space coordinate system P-xyz;

[0046] Step 9: Take the twisted line as the system control object. According to the desired motion state of the control target of the twisted line drive system, that is, the desired speed, force, and specified displacement of the twisted line, use the non-linear variation relationship of F = Fcn(θ, ω, T, W, Y) to feedback-regulate the motor rotation angle θ and rotation speed ω, so that the system output reaches the desired target, thereby realizing the motion control of the twisted line drive system. Among them, Fcn represents the functional relationship between the motor rotation speed ω, rotation angle θ and three screws in the twisted line drive system.

[0047] An electronic device of the present invention includes a memory and a processor, characterized in that the memory is used to store a program for supporting the processor to execute the motion control method, and the processor is configured to execute the program stored in the memory.

[0048] A computer-readable storage medium of the present invention, characterized in that a computer program is stored on the computer-readable storage medium, and when the computer program is run by a processor, it executes the steps of the motion control method.

[0049] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0050] 1. The present invention makes full use of the projective space and Planck coordinates to accurately describe the spatial position relationship of the twisted line in the twisted line drive system, expands and extends the coordinate system on the basis of the ordinary three-dimensional coordinates, effectively solves the problems of unclear and ambiguous traditional space calibration, and improves the accuracy and rapidity of the twisted line drive system to obtain position parameters.

[0051] 2. Based on screw algebra and screw displacement theory, the present invention uses the Chasles motion decomposition method to decompose the motion process of the twisted wire drive system into rotational motion about an axis and axial translational motion. The screw operation method is used to optimize the calculation process of the flexible drive motion, more directly obtain the motion state parameters, and achieve accurate force-position analysis of the twisted wire drive system.

[0052] 3. The present invention uses the rotation angle and angular velocity of the motor, and through collaborative calculation with the spatial parameters of the current position of the system, the screw state matrix of the system motion is calculated, including velocity screw, force screw and position screw. Thus, the motion state control function of the system is given, and the effective decomposition analysis of the torsional motion and axial motion of the twisted wire drive system is realized. The control of the motion process is more accurate, the description of the motion state is clearer, the generation of control errors is greatly reduced, and the stability and accuracy of the control are improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] Figure 1 is a flowchart of the motion control method for the twisted wire drive system of the present invention;

[0054] Figure 2 is an overall architecture diagram of the twisted wire drive system of the present invention;

[0055] Figure 3 is an analysis diagram of the five-dimensional projective space of the twisted wire drive system of the present invention;

[0056] Figure 4 is a motion decomposition diagram of the twisted wire drive system;

[0057] Figure 5 is a motion control relationship diagram of the twisted wire drive system. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0058] In this embodiment, a motion control method based on a twisted wire drive system, as Figure 1 shown, is carried out according to the following steps:

[0059] Step 1: As Figure 2 shown, to accurately and quickly obtain the position parameters of the twisted wire in space, a five-dimensional projective space coordinate system P 5 of the twisted wire drive system is constructed:

[0060] Taking the end of the rotation center axis of the twisted wire drive system as the origin O, the direction parallel to the rotation center axis as the Z axis, a straight line perpendicular to the Z axis and passing through the origin O as the X axis, and a straight line perpendicular to the X axis and Z axis and passing through the origin O as the Y axis, a global space coordinate system O-XYZ is established;

[0061] Taking an arbitrary position point on the torsion wire in the torsion wire drive system as the origin P, the straight line parallel to the Z-axis and passing through the origin P as the z-axis, the straight line parallel to the X-axis and passing through the origin P as the x-axis, and the straight line perpendicular to the x- and z-axes and passing through the origin P as the y-axis, a projective space coordinate system P-xyz is established;

[0062] The five-dimensional projective space coordinate system P is composed of the global space coordinate system O-XYZ and the projective space coordinate system P-xyz 5 ;

[0063] Step 2: As Figure 3 shown, establish the Plücker coordinates of the torsion wire at the origin P of the projective space coordinate system P-xyz;

[0064] Step 2.1: Assume that the torsion wire is at the point P' above the origin P. Denote the coordinates of the origin P and the point P' in the global coordinate system O-XYZ as P(x1, y1, z1) and P'(x2, y2, z2) respectively. Use a high-speed camera to collect the motion image of the system at the current moment, and obtain the current spatial position of the control object with the help of general measurement software, get the position coordinates of the research point in the projective space, and use the attitude vector between the origin P and the point P' as the direction of the torsion wire motion vector. Then use Equation (1) to obtain the attitude vector of the torque on the origin O of the global space coordinate system O-XYZ

[0065]

[0066] In Equation (1), p, q, and r respectively represent the components of the attitude vector of the torque taken on the origin O of the global coordinate system O-XYZ in the X-axis, Y-axis, and Z-axis directions, and p = y1z2 - y2z1, q = x2z1 - x1z2, r = x1y2 - x2y1, and where r1 and r2 respectively represent the vectors pointing from the origin O to the points P and P';

[0067] Step 2.2: In the global space coordinate system O-XYZ, use Equation (2) to obtain the Plücker coordinates of the torsion wire

[0068]

[0069] As Figure 4As shown, the kinematic decomposition of the wire twisting drive is carried out, and the wire twisting motion is decomposed into rotational motion around the axis and translational motion along the axis. The wire in the wire twisting drive system is equivalent to a helical rigid body. The displacement of the rigid body can be regarded as the displacement of a triad of "points on a directed line segment on a directed plane", or the motion of the rigid body can be regarded as the motion of a straight line and a point outside the straight line. All finite motions and infinitesimal motions of the rigid body in space can be equivalently regarded as rotation around the axis and translation along the axis.

[0070] The velocity screw is a screw containing the velocity amplitude, belonging to the category of moment quantities and also an element of the Lie algebra se(3), denoted by T. Its axis is the axis of the rotation axis, and the direction of translation is parallel to this axis. The velocity screw is used to describe the velocity state of the controlled object in space, and its basic calculation steps are as follows.

[0071] Step 3: Use Equation (3) and Equation (4) to obtain the descriptions of the angular velocity and linear velocity of the wire in the global space coordinate system O-XYZ, and thus obtain the velocity screw matrix T of the wire in the wire twisting drive system from Equation (5):

[0072]

[0073]

[0074]

[0075] In Equation (3) and Equation (4), α represents the helix angle of the wire in the wire twisting drive system, and β represents the angle between the projection of the line segment PP′ between the origin P and point P′ on the projective plane P-xy and the x-axis direction; ω x 、ω y 、ω z respectively represent the components of the resultant angular velocity ω′ of the wire on the x-axis, y-axis, and z-axis, and ν x 、ν y 、ν z respectively represent the components of the resultant velocity ν′ of the wire on the x-axis, y-axis, and z-axis;

[0076] In Equation (5), ω represents the angular velocity of the wire in the projective plane P-xy, and its vector direction points from the origin P to the projection point of P′ on the projective plane, and the scalar magnitude ||ω|| is equal to the motor torsional angular velocity ω t ; ω′ and ν′ respectively represent the resultant angular velocity and resultant velocity of the wire, and their vector directions are both the same as the direction, and where h is the velocity screw moment, which is the ratio of the translational velocity to the rotational velocity, that is ν represents the velocity of the wire in the projective plane P-xy, and its vector direction is the same as ω;

[0077] The force screw matrix consists of a pure force and a couple of forces parallel to the line of action of the force. It is a screw containing the force amplitude and forms a dual relationship with the Lie algebra se(3). It is denoted by W. Its axis direction is similar to that of the velocity screw. The force screw is used to describe the force state of the controlled object in the projective space. The basic calculation steps are as follows.

[0078] Step 4: Decompose the force on the controlled object of the twisted wire drive system, i.e., the twisted wire, using the theory of general physics. According to the screw geometry theory and the Poinsot central axis theorem, any force system of a force and a couple of forces can be simplified to a pure force at a certain fixed point in space and a couple of forces parallel to it. Then, obtain the force screw matrix W of the twisted wire in the twisted wire drive system based on the decomposition of the resultant force and the resultant moment:

[0079] Step 4.1: Obtain the resultant force vector acting on the twisted wire in the twisted wire drive system using Equation (6)

[0080]

[0081] In Equation (6), ||f′|| represents the scalar magnitude of the resultant force acting on the twisted wire in the twisted wire drive system, and ||f′|| = ||f|| / cosα. ||f|| represents the scalar of the force acting on the projective plane of the twisted wire and is calculated from the angular velocity ω of the motor directly set by the motor control unit t and the rotation radius r of the twisted wire drive system;

[0082] Step 4.2: Obtain the resultant couple vector using Equations (8) and (9) The two decomposed components:

[0083]

[0084]

[0085] In Equations (8) and (9), represents the component in the same direction as and represents the component orthogonal to . c represents the resultant couple vector projected in the direction, represents the vector between the intersection point of the projective plane P-xy and the central rotation axis of the twisted wire drive system and the origin P of the projective space coordinate system P-xyz it points to;

[0086] Step 4.3: Obtain the force screw matrix W of the twisted wire in the twisted wire drive system using Equation (10)

[0087]

[0088] In Equation (10), represents the resultant force vector acting on the twisted wire, represents the resultant couple vector of the twisted wire with respect to the origin P of the projective space coordinate system P-xyz, and

[0089] The displacement of a straight line in space can be described by the displacements of two points, while the displacement of a rigid body can be described by three points not on the same straight line. The most effective method is to use the screw displacement operator. The operation of the finite displacement screw can be represented by a finite displacement screw matrix with the adjoint action of the Lie group SE(3). As the adjoint representation of the Lie group, the finite displacement screw matrix has a 3*3 dual matrix form and a 6*6 matrix form, which can be used to describe the rotation and translation of rigid body motion and Chasles motion. Any composite finite displacement screw can be determined by the rotation angle, the pose of the rotation axis, and the screw moment or the translation distance along the axis. The rotation angle is determined and generally limited to the range [-π, π]. The finite displacement screw is used to describe the displacement of the control object of the twisted wire drive system in the projective space, and its basic calculation steps are as follows.

[0090] Step 5: Obtain the rotation matrix R of the twisted wire drive system using Equation (11):

[0091] R = I + sinθ·A s +(1 - cosθ)·A s ·A s (11)

[0092] In Equation (11), I is the identity matrix, and A s is the skew-symmetric matrix of where s x 、s y 、s z represent the components of the rotation center axis in the x-axis, y-axis, and z-axis directions respectively; θ is the motor rotation angle;

[0093] Step 6: According to the translation vector of the twisted wire drive system, use Equation (12) to obtain the matrix A that realizes the change in the position of the screw axis:

[0094]

[0095] In Equation (12), d x 、d y 、d z represent the components of the translation vector of the twisted wire drive system in the x-axis, y-axis, and z-axis directions respectively;

[0096] Step 7: Use Equation (13) and Equation (14) to obtain the final pose of the twisted wire in the twisted wire drive system after rotation and translation motions respectively and the translation modulus length ι of the twisted wire along the central rotation axis:

[0097]

[0098]

[0099] In Equation (13) and Equation (14), represents the Plücker coordinates of the twisted wire, i.e., the initial pose vector of the twisted wire; represents the final pose of the twisted wire after rotation and translation motions; tr represents the trace of the matrix;

[0100] Step 8: Use Equation (14) to obtain the finite displacement screw matrix Y of the twisted wire in the twisted wire drive system:

[0101]

[0102] In Equation (14), represents the vector pointing from the origin O of the global space coordinate system O-XYZ to the origin P of the projective space coordinate system P-xyz;

[0103] Step 9: The control flow of the twisted wire drive system is as shown in Figure 5 According to the motion state expected to be achieved by the control target of the twisted wire drive system, use the nonlinear variation relation of F = Fcn(θ, ω, T, W, Y) to perform feedback regulation on the motor rotation angle θ and rotational speed ω t so as to realize the motion control of the twisted wire drive system, where Fcn represents the functional relation between the motor rotational speed ω t and rotation angle θ and three kinds of screws in the twisted wire drive system.

[0104] For example, by inputting the torsional angular velocity ω t of the motor, given a motor torsional angle θ, obtain the position parameters of the twisted wire in the five-dimensional projective space coordinate system through the photos taken by the high-speed camera, and calculate the Plücker coordinates of the twisted wire Different from the position parameters given by the general three-dimensional space coordinate system adopted by the traditional calculation method, the five-dimensional projective space coordinate is in a superposition-like manner on the basis of the three-dimensional space, which expands the coordinate system and adds a projective space coordinate system. The six-dimensional position coordinate parameters obtained by using this coordinate system can more accurately describe the position of the twisted wire in the twisted wire drive system and are more beneficial to the calculation of speed parameters, force parameters and displacement parameters.

[0105] After obtaining the position parameters, based on the Pythagorean theorem and general geometric methods, common relational expressions for velocity and force decomposition are obtained. Further, using the spinor algebra method, the angular velocity ω of the current input motor is calculated. t For the velocity spinor, force spinor, and displacement spinor of the lower torsional wire, compared with the traditional calculation method, more complete state description equations are obtained in the axial direction and the direction of rotation around the axis of the torsional wire drive system. An intuitive functional relationship between the angular velocity of the motor and such parameters is simplified to determine whether the torsional wire drive system has reached the state expected by the present invention, and in turn, the motor is adjusted to finally reach the target state. Using this method, the feedback adjustment process is greatly simplified, and the response speed of the system is significantly improved.

Claims

1. A motion control method based on a twisted wire drive system, characterized in that it is Proceed as follows: Step 1: Construct the five-dimensional projective space coordinate system P of the twisting wire drive system 5 : Taking the end of the rotation center axis of the torsion wire drive system as the origin O, the direction parallel to the rotation center axis as the Z-axis, a straight line perpendicular to the Z-axis and passing through the origin O as the X-axis, and a straight line perpendicular to the X-axis and Z-axis and passing through the origin O as the Y-axis, establish a global space coordinate system O-XYZ; Taking any position point on the torsion wire in the torsion wire drive system as the origin P, the straight line parallel to the Z-axis and passing through the origin P as the z-axis, the straight line parallel to the X-axis and passing through the origin P as the x-axis, and a straight line perpendicular to the x-axis and z-axis and passing through the origin P as the y-axis, establish a projective space coordinate system P-xyz; A five-dimensional projective space coordinate system P is formed by the global space coordinate system O-XYZ and the projective space coordinate system P-xyz 5 ; Step 2: Establish the Plücker coordinates of the torsion wire at the origin P of the projective space coordinate system P-xyz; Step 2.1: Assume that the twisted wire is at point P' above the origin P. Denote the coordinates of the origin P and point P' in the global coordinate system O-XYZ as P(x1, y1, z1) and P'(x2, y2, z2) respectively, and use the attitude vector between the origin P and point P' as the direction of the motion vector of the twisted wire, and obtain the said attitude vector by using Equation (1) the torque on the origin O of the global space coordinate system O-XYZ In formula (1), p, q, and r respectively represent the attitude vector The torque taken about the origin O of the global coordinate system O-XYZ The components in the X-axis, Y-axis, and Z-axis directions, and p = y1z2 - y2z1, q = x2z1 - x1z2, r = x1y2 - x2y1; Step 2.2: In the global space coordinate system O-XYZ, obtain the Plücker coordinates of the twisted line using Equation (2) Step 3: Use Equation (3) and Equation (4) to obtain the descriptions of the angular velocity and linear velocity of the torsion wire in the global space coordinate system O-XYZ, and thus obtain the velocity screw matrix T of the torsion wire in the torsion wire drive system from Equation (5); In Equations (3) and (4), α represents the helix angle of the twisted wire in the twisted wire drive system, and β represents the angle between the projection of the line segment PP' between the origin P and point P' on the projective plane P-xy and the x-axis direction; ω x 、ω y 、ω z respectively represent the components of the resultant angular velocity ω' of the twisted wire on the x-axis, y-axis, and z-axis, and ν x 、ν y 、ν z respectively represent the components of the resultant velocity ν' of the twisted wire on the x-axis, y-axis, and z-axis; In formula (5), ω represents the angular velocity of the torsion line in the projective plane P-xy. Its vector direction points from the origin P to the projection point of P' in the projective plane, and the scalar magnitude ||ω|| is equal to the motor torsion angular velocity ω t ; ω' and ν' respectively represent the combined angular velocity and combined velocity of the torsion line, and their vector directions are both in the same direction; ν represents the velocity of the torsion line in the projective plane P-xy, and its vector direction is the same as ω; Step 4: Obtain the force screw matrix W of the torsion wire in the torsion wire drive system; Step 4.1: Obtain the resultant force vector acting on the twisted wire in the twisted wire drive system using Equation (6) In Equation (6), ||f′|| represents the scalar magnitude of the resultant force on the torsion wire in the torsion wire drive system, and ||f′|| = ||f|| / cosα, where ||f|| represents the scalar of the force received by the projection plane of the torsion wire and is calculated from the angular velocity ω of the motor t and the rotation radius r of the torsion wire drive system; Step 4.2: Obtain the resultant couple vector using equations (8) and (9) The two components after decomposition: In Equations (8) and (9), represents the component in the same direction as ; represents the component orthogonal to ; c represents the resultant couple vector projected in the direction of ; represents the vector between the origin P of the projective space coordinate system P - xyz, which is pointed to by the intersection point of the projective plane P-xy and the central rotation axis of the torsion drive system, and Step 4.3: Use Equation (10) to obtain the force screw matrix W of the torsion wire in the torsion wire drive system; In Equation (10), represents the resultant force vector acting on the twisted wire, represents the resultant couple vector of the twisted wire with respect to the origin P of the projective space coordinate system P-xyz; Step 5: Use Equation (11) to obtain the rotation matrix R of the torsion wire drive system; R = I + sinθ·A s +(1 - cosθ)·A s ·A s (11) In Equation (11), I is the identity matrix, and A s is the skew-symmetric matrix of, where is the central axis vector of the rotation axis, and s x s y s z respectively represent the components of the rotation central axis in the x-axis, y-axis, and z-axis directions; θ is the motor rotation angle; Step 6: According to the translation vector of the wire twisting drive system Use Equation (12) to obtain the matrix A that realizes the change in the position of the screw axis: In formula (12), d x , d y , d z respectively represent the components of the translation vector of the torsion drive system in the x-axis, y-axis, and z-axis directions; Step 7: Use Equation (13) and Equation (14) to obtain the final pose of the twisted wire in the twisted wire drive system after rotational and translational motions respectively and the translational modulus length ι of the twisted wire along the central rotation axis: In formulas (13) and (14), represents the Plücker coordinates of the twisted line, that is, the initial pose vector of the twisted line; represents the final pose of the twisted line after rotation and translation; tr represents the trace of the matrix; Step 8: Use Equation (14) to obtain the finite displacement screw matrix Y of the torsion wire in the torsion wire drive system; In Equation (14), represents the vector pointing from the origin O of the global space coordinate system O-XYZ to the origin P of the projective space coordinate system P-xyz; Step 9: Take the torsion wire as the system control object. According to the desired motion state to be achieved by the control target of the torsion wire drive system, that is, the desired speed, force, and specified displacement of the torsion wire, use the nonlinear variation relationship of F = Fcn(θ, ω, T, W, Y) to feedback-regulate the motor rotation angle θ and rotation speed ω, so that the system output reaches the desired target, thereby realizing the motion control of the torsion wire drive system, where Fcn represents the functional relationship between the motor rotation speed ω, rotation angle θ and the three screw quantities in the torsion wire drive system.

2. An electronic device, comprising a memory and a processor, characterized in that, The memory is used to store a program for supporting the processor to execute the motion control method described in Claim 1, and the processor is configured to execute the program stored in the memory.

3. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is run by the processor, it executes the steps of the motion control method described in Claim 1.

Images