An impedance control method

Abstract

This application discloses an impedance control method, comprising: performing dynamic modeling to obtain a closed-form dynamic equation in joint space; obtaining an expression for the driving force based on the closed-form dynamic equation; obtaining the relationship between the rope velocity vector, rope tension vector, joint velocity vector, and joint equivalent torque based on the principle of virtual work; calculating the rope tension from the joint torque, where the driving force solution can be expressed as the sum of a least-squares solution and an arbitrary null-space solution; analyzing the joint torque output capacity under rope tension constraints; establishing an ideal mathematical model for the tension transmission process of the rope-driven joint and analyzing the transient coupling effect under rope tension control in joint space; performing joint motion analysis under joint tension control in joint space; and establishing a target impedance model in joint space. This application effectively eliminates coupling phenomena in the tension control process and improves the active compliance of a rope-driven serpentine manipulator.

Description

Technical Field

[0001] The present application relates to the field of compliance control technology, and in particular to an impedance control method. Background Art

[0002] Force compliance control can be roughly divided into two categories according to its implementation method: based on position inner loop control, that is, admittance model; based on force inner loop control, that is, impedance model.

[0003] Currently, most research focuses on admittance-based compliant control based on external force observation, while impedance control methods have been less discussed, particularly for multi-degree-of-freedom rope-driven serpentine manipulators. Under the admittance model, the position of the manipulator is controlled according to a certain compliance strategy using known external forces. The most direct approach is to directly measure the external forces using sensors. For example, a six-dimensional force sensor is installed at the end of the rope-driven manipulator to measure the external forces. The desired joint positions are then determined using an admittance model and inverse kinematics. A position inner loop, consisting of a hybrid control of rope force or rope position, is then used to achieve highly accurate compliant control of the end of the rope-driven serpentine manipulator. However, this approach requires the installation of large and heavy force sensors, making it unsuitable for most small, low-load rope-driven serpentine manipulators. Summary of the Invention

[0004] To solve at least one of the above technical problems, the present application provides an impedance control method, the technical solution adopted is as follows:

[0005] The present application provides an impedance control method, which includes:

[0006] Perform dynamic modeling, organize the recursive form dynamic equations, and obtain the closed form dynamic equations in the joint space;

[0007] According to the closed-form dynamic equation, the expression of the driving pulling force is obtained;

[0008] According to the principle of virtual work and the mapping relationship between joints and rope lengths, the relationship between rope velocity vector, rope tension vector, joint velocity vector, and joint equivalent torque is obtained.

[0009] The rope tension is calculated by the joint torque, and the driving tension solution can be expressed as the sum of the least squares solution and any null space solution;

[0010] Analyze the joint torque output capacity under rope tension constraint;

[0011] An ideal mathematical model for the tension transmission process of the rope-driven joint is established to analyze the transient coupling effect under the control of rope spatial tension.

[0012] Conduct joint motion analysis under joint space tension control;

[0013] Establish the target impedance model in the joint space.

[0014] In certain embodiments of the present application, the performing kinetic modeling comprises:

[0015] The force balance equation for the end link is established, and the equivalent driving torque of the universal joint is obtained by combining the direction of the joint axis defined by the DH coordinate system.

[0016] The force balance equations for the other links are established, and the equivalent driving torque of the universal joint is obtained by combining the direction of the joint rotation axis defined by the DH coordinate system.

[0017] In certain embodiments of the present application, the expression for obtaining the driving pulling force includes:

[0018] Find a feasible solution through the rope tension distribution algorithm;

[0019] The scalar mapping relationship between the driving rope tension and the equivalent joint driving torque is obtained according to the rope and joint space mapping matrix.

[0020] In certain embodiments of the present application, the driving force solution can be expressed as the sum of a least squares solution and an arbitrary null space solution including:

[0021] The internal tension coefficient is selected. The internal tension is located in the right null space of the Jacobian matrix of the rope length function at the joint segment. The internal tension coefficient is selected arbitrarily.

[0022] In certain embodiments of the present application, the selected internal tension coefficient includes:

[0023] Find the minimum internal tension of the rope so that all rope tensions are greater than the lower limit of tension and obtain the minimum solution of the total tension;

[0024] Calculate the internal tension coefficient that makes the tension of each driving rope exactly equal to the lower limit of tension, and take the maximum value among them;

[0025] Make the minimum tension in the drive rope exactly equal to the lower limit of tension.

[0026] In certain embodiments of the present application, the analyzing the joint torque output capability under rope tension constraint includes:

[0027] Establish a coordinate system to decompose the output torque onto the orthogonal universal joint axes;

[0028] The output torque direction is obtained from the universal joint torque;

[0029] Based on the rope tension constraint and the output torque constraint, an optimization function can be constructed to obtain the maximum output torque under a given joint position and a given output torque direction.

[0030] In certain embodiments of the present application, the analyzing the joint torque output capability under rope tension constraint further includes:

[0031] Assuming that each driving rope passes through the joint rope hole near the base end at the maximum envelope angle and assuming that static friction is used, the maximum tension variation coefficient is obtained;

[0032] According to the maximum pulling force variation coefficient, the actual driving pulling force is obtained after the friction force acts.

[0033] In certain embodiments of the present application, an ideal mathematical model is established for the tension transmission process of the rope-driven joint, and the transient coupling effect under the control of the rope spatial tension is analyzed, including:

[0034] Ignore the friction in the transmission, transfer the mass of the transmission component to the slider, and simplify the model to a slider driving the rope;

[0035] Make assumptions about the model of the rope-driven joint;

[0036] According to the tension transmission model, the relationship between the output force of the driving motor and the movement of the slider is obtained;

[0037] According to the rope stiffness model, the rope tension is obtained by the position difference between the slider and the joint angle.

[0038] In certain embodiments of the present application, establishing an ideal mathematical model for the tension transmission process of the rope-driven joint and analyzing the transient coupling effect under the rope spatial tension control further includes:

[0039] Use PD control in the closed-loop control of rope tension to obtain the desired motor current;

[0040] Get the relationship between joint motion and desired torque input;

[0041] The reasons causing the transient coupling phenomenon are analyzed.

[0042] In certain embodiments of the present application, the performing of joint motion analysis under joint space tension control includes:

[0043] By comparing the expected and equivalent actual output torques, a closed loop of joint torque control is formed;

[0044] The driving current signal of the motor is obtained through spatial conversion of joints and ropes to weaken the coupling effect of joint motion.

[0045] The embodiments of the present application have at least the following beneficial effects: in the present application, impedance control has the advantages of high dynamic response and low jitter compared with the admittance scheme; the impedance control method mainly consists of three parts: dynamic modeling, rope tension distribution and control, and joint impedance control method; in order to simplify the modeling difficulty, the force acting on the rigid link is divided into adjacent link force, inertia force and rope force according to the object of action; when actually controlling the rope-driven arm, it is necessary to calculate the control input based on the motion parameters collected in real time, so that the actual motion of the rope-driven arm tracks the expected motion trajectory; the coupled motion generated between different joints is analyzed, and joint space control is proposed to eliminate the coupling phenomenon; the joint impedance control is studied to improve the compliance of the rope-driven manipulator; the impedance control method can effectively eliminate the coupling phenomenon in the tension control process and improve the active compliance of the rope-driven serpentine manipulator.

[0046] Additional aspects and advantages of the present application will be given in part in the description below, and in part will become obvious from the description below, or will be learned through practice of the present application. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] The above and / or additional aspects and advantages of the present application will become apparent and easily understood from the description of the embodiments in conjunction with the following drawings, in which:

[0048] Figure 1 This is a flow chart of an impedance control method provided by an embodiment of the present application;

[0049] Figure 2 is a flow chart of an impedance control method provided by another embodiment of the present application;

[0050] Figure 3 is a flow chart of an impedance control method provided by another embodiment of the present application;

[0051] Figure 4 is a flow chart of an impedance control method provided by another embodiment of the present application;

[0052] Figure 5 is a flow chart of an impedance control method provided by another embodiment of the present application;

[0053] Figure 6 is a flow chart of an impedance control method provided by another embodiment of the present application;

[0054] Figure 7 is a flow chart of an impedance control method provided by another embodiment of the present application;

[0055] Figure 8 is a flow chart of an impedance control method provided by another embodiment of the present application;

[0056] Figure 9is a flow chart of an impedance control method provided by another embodiment of the present application;

[0057] Figure 10 is a flow chart of an impedance control method provided by another embodiment of the present application;

[0058] Figure 11 It is a schematic diagram of the direction of the joint output torque in this application;

[0059] Figure 12 It is a simplified model of rope-driven joint tension transmission in this application;

[0060] Figure 13 This is the flow chart of rope space tension control in this application;

[0061] Figure 14 It is the joint space tension control flow chart in this application;

[0062] Figure 15 This is the joint impedance control flow chart in this application. DETAILED DESCRIPTION

[0063] This section will combine Figures 1 to 15 Embodiments of the present application are described in detail, and examples of the embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present application, and are not to be construed as limiting the present application.

[0064] In the description of this application, it should be understood that if the terms "center", "middle", "longitudinal", "lateral", "length", "width", "thickness", "up", "down", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inside", "outside", "axial", "radial", "circumferential" and the like appear, the orientation or position relationship indicated is based on the orientation or position relationship shown in the drawings, which is only for the convenience of describing this application and simplifying the description, and does not indicate or imply that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as a limitation on this application. Features defined as "first" and "second" are used to distinguish feature names, rather than having special meanings. In addition, features defined as "first" and "second" may explicitly or implicitly include one or more of the features. In the description of this application, unless otherwise specified, "multiple" means two or more.

[0065] In the description of this application, it should be noted that, unless otherwise expressly specified or limited, the terms "mounted," "connected," and "connected" should be understood in a broad sense. For example, they can refer to fixed connections, detachable connections, or integral connections; mechanical connections or electrical connections; direct connections or indirect connections through an intermediate medium; and internal connections between two components. Those skilled in the art will understand the specific meanings of the above terms in this application based on the specific circumstances.

[0066] The embodiments of the present application provide an impedance control method, which is applied to a rope-driven manipulator. Various embodiments of the impedance control method of the present application are presented below.

[0067] like Figure 1 As shown, Figure 1 This is a flowchart of an impedance control method provided by an embodiment of the present application. The impedance control method includes but is not limited to step S110, step S120, step S130, step S140, step S150, step S160, step S170 and step S180.

[0068] Step S110 , performing dynamic modeling, sorting out the recursive form dynamic equation, and obtaining the closed form dynamic equation in the joint space.

[0069] The dynamic analysis of a rope-driven manipulator is complicated by the coupling between the rigid links and the flexible rope. To simplify modeling, the forces acting on the rigid links are divided into forces acting on adjacent links, inertial forces, and rope forces. An iterative dynamic equation for the manipulator is established based on the Newton-Euler equations. The recursive form of the dynamic equation is then reorganized to obtain a closed-form dynamic equation.

[0070] In addition, if Figure 2 As shown, in one embodiment, step S110 may include but is not limited to the following steps:

[0071] Step S210: Establish a force balance equation for the end link, and obtain the equivalent driving torque of the universal joint based on the joint rotation axis direction defined by the DH coordinate system;

[0072] Step S220 , establishing a force balance equation for the other connecting rods, and combining the joint rotation axis direction defined by the DH coordinate system to obtain the equivalent driving torque of the universal joint.

[0073] First, the velocity and acceleration of the rope-driven manipulator are forward recursively calculated. According to the motion of each joint, the center of mass coordinate system of each link {c i Linear acceleration and angular acceleration of The end link has only one adjacent link, unlike the other links. Therefore, it is necessary to establish force balance equations for the end link and the other links separately.

[0074] For the end link N, there is the following force balance equation:

[0075]

[0076]

[0077] in, Indicates the centroid coordinate system {c N The rotation matrix of the {2N} coordinate system relative to the {2N} coordinate system, They are respectively {c N}Inertial force and moment of inertia at the center of mass, They represent the resultant force and torque of the driving rope on the end link N in the {2N} coordinate system, 2N f N , 2N n N They represent the support force and moment of the N-1th link on the Nth link in the {2N} coordinate system, In the coordinate system {2N}, the origin of {2N-2} points to the center of mass c. N The position vector of .

[0078] Combined with the joint axis direction defined by the DH coordinate system, the equivalent driving torque τ of the universal joint is N It can be calculated by the following formula:

[0079]

[0080] Among them, the driving torque They are The y-axis and z-axis components of is the moment of inertia of the center of mass {2i} in the center of mass coordinate system of the end link N.

[0081] For the remaining connecting rods i, the same force balance equation applies:

[0082]

[0083]

[0084] in, Indicates the centroid coordinate system {c i The rotation matrix of the {2i} coordinate system relative to the {2i} coordinate system, 2i R 2i+2 represents the rotation matrix of the {2i+1} coordinate system relative to the {2i} coordinate system in the center of mass coordinate system, They are respectively {c i}Inertial force and moment of inertia at the center of mass, In the coordinate system {2i}, the origin of {2i-2} points to the center of mass c. i The position vector of They represent the resultant force and moment of the rope acting on the i-th link in the {2i} coordinate system, denote the resultant force and torque of the driving rope on the i-th link in the {2i} coordinate system, 2i f i , 2i n i They represent the support force and moment of the i-1th link on the i-th link in the {2i} coordinate system, 2i+2 f i+1 , 2i+ 2 n i+1 are the reaction force and torque of the i+1th link on the ith link in the {2i+2} coordinate system.

[0085] By knowing the reaction force and moment from the i+1th link 2i+2 f i+1 、 2i+2 n i+1 and the resultant torque of all the ropes passing through The above formula can be solved to get Thus, the equivalent driving torque τ in the DH coordinate system is obtained i .

[0086] In order to conveniently represent the dynamic model of the rope-driven manipulator, the derived recursive form dynamic equation is sorted out to obtain the closed form dynamic equation in the joint space:

[0087]

[0088] Where M, C, g are mass, centripetal force\Coriolis force and gravity matrix respectively; q i , are joint angle, joint angular velocity and joint angular acceleration respectively; τ e The torque generated by the external force in the joint space is called the external force torque, and τ is the equivalent driving torque of the joint.

[0089] The mapping of the joint equivalent driving torque τ and the driving rope tension f can be organized into the following matrix form:

[0090] τ=R T f,

[0091] Among them, R T is the mapping matrix between the driving rope tension and the equivalent driving torque.

[0092] Step S120: Obtain an expression for the driving force according to a closed-form dynamic equation.

[0093] When controlling a rope-driven manipulator, it is necessary to calculate the control input based on the real-time motion parameters. This involves calculating the driving rope tension so that the actual motion of the rope-driven manipulator tracks the desired trajectory. The process of deriving the applied force from the manipulator's motion is known as inverse dynamics. Using the closed-form dynamics equation, the driving tension can be expressed as:

[0094]

[0095] Among them, R T + is the pseudo-inverse of the mapping matrix between the driving rope tension and the equivalent driving torque.

[0096] In addition, if Figure 3 As shown, in one embodiment, step S120 may include but is not limited to the following steps:

[0097] Step S310, finding a feasible solution through a rope tension distribution algorithm;

[0098] Step S320 , obtaining a scalar mapping relationship between the driving rope tension and the equivalent joint driving torque according to the rope-joint space mapping matrix.

[0099] Due to the redundant driving characteristics of the rope-driven joint, three ropes are used to drive the two-degree-of-freedom joint. Therefore, there are infinite solutions for the driving rope tension, and a feasible solution needs to be found through the rope tension distribution algorithm.

[0100] To simplify the mapping matrix R T The more complex space vector operations in the MATLAB® software can be used to obtain the scalar mapping relationship between the driving rope tension and the equivalent joint driving torque with the help of the rope-joint space mapping matrix.

[0101] Step S130, based on the principle of virtual work and the mapping relationship between joints and rope lengths, the relationship between the rope velocity vector, the rope tension vector, the joint velocity vector, and the joint equivalent torque is obtained;

[0102] In step S140 , the rope tension is calculated using the joint torque. The driving tension solution can be expressed as the sum of the least squares solution and an arbitrary null space solution.

[0103] First, consider the single-rope-driven joint model. According to the principle of virtual work, the work done by the rope tension is equal to the work done by the external torque:

[0104]

[0105] in, is the velocity vector of the driving rope of the i-th joint, is the tension vector of the driving rope of the i-th joint, is the velocity vector of the i-th joint, τ i is the equivalent driving torque of the i-th joint.

[0106] Combining the mapping relationship from joints to rope lengths, we can get:

[0107]

[0108] Among them, R i，i is the Jacobian matrix of the rope length function of the i-th group of ropes at the i-th joint segment.

[0109] The rope tension can be calculated from the joint torque by the inverse operation. i，i T For a wide matrix, the driving force solution can be expressed as the sum of the least squares solution and an arbitrary null space solution:

[0110]

[0111] Among them, N i =null(R i，i T ) whose column space is the matrix R i，i T The right null space of R i，i T N i =0;t i is the internal tension coefficient, the product term N i t i represents the internal tension vector of the driving rope. N i can be chosen as a unit vector, then N i -1 =N i T , then H -1 =[(R i，i T ) + N i ].

[0112] In addition, if Figure 4 As shown, in one embodiment, step S140 may include but is not limited to the following steps:

[0113] Step S410: Select an internal tension coefficient. The internal tension is located in the right null space of the Jacobian matrix of the rope length function at the joint segment. The internal tension coefficient is arbitrarily selected.

[0114] Since the internal tension is located at R i，i TThe right null space of , so the internal tension coefficient t i It can be selected arbitrarily without affecting the output torque. Usually the rope tension obtained by the least square solution is not necessarily all positive tension. In order to prevent the rope tension from being too small and causing the rope to slack, the internal tension coefficient t can be adjusted. i , so that the driving rope is added with a positive internal tension, and it is greater than the set minimum value to ensure that the rope is in a taut state.

[0115] In addition, if Figure 5 As shown, in one embodiment, step S410 may include but is not limited to the following steps:

[0116] Step S510, finding the minimum internal tension of the rope so that all rope tensions are greater than the lower limit of tension, and obtaining the minimum solution of the total tension;

[0117] Step S520, finding the internal tension coefficients that make the tension of each drive rope just equal to the tension lower limit, and taking the maximum value thereof;

[0118] Step S530: Make the minimum tension in the driving rope just equal to the lower limit of tension.

[0119] For the internal tension coefficient t i There are many ways to select . From the perspective of minimizing the driver output, the minimum internal rope tension can be obtained so that all rope tensions are greater than the lower limit of tension, and the minimum solution for tension and tension can be obtained:

[0120]

[0121] Among them, f min is the given minimum positive tension of the rope. Here, it is assumed that all ropes are the same and use the same value; a k is the least squares solution of the rope tension k, b k is the null space component of rope k.

[0122] Under the minimum rope tension constraint, the above equation calculates the internal tension coefficients that ensure that the tension of each driving rope is exactly equal to the tension lower limit. The maximum of these coefficients is taken, ensuring that the minimum tension in the driving rope is exactly equal to the tension lower limit. This coefficient is the minimum positive internal tension coefficient, and the resulting tension is the solution for the minimum tension sum. The tension distribution algorithm can ensure that the minimum rope tension meets the tension lower limit constraint by calculating the internal tension coefficients. However, the difference between the driving rope tensions is determined by the magnitude of the given output torque. Therefore, if the rope tension exceeds the upper limit, it indicates that the given output torque exceeds the torque output capacity of the joint under the tension constraint.

[0123] Step S150 , analyzing the joint torque output capability under the rope tension constraint.

[0124] To ensure that the rope tension remains within the safety constraints during the operation of the manipulator, it is necessary to constrain the maximum given joint torque. Therefore, it is necessary to analyze the joint torque output capacity under certain rope tension constraints.

[0125] Because the driving force arm of the cable tension changes with the joint position, the joint's ultimate torque output varies at different joint angles under a given cable tension constraint. Furthermore, due to the distribution of the joint's driving cables, the ultimate torque output along different directions at the same joint angle can also vary.

[0126] In addition, if Figure 6 As shown, in one embodiment, step S150 may include but is not limited to the following steps:

[0127] Step S610: establishing a coordinate system to decompose the output torque onto the orthogonal universal joint shafts;

[0128] Step S620, obtaining the output torque direction from the universal joint torque;

[0129] In step S630 , an optimization function can be constructed based on the rope tension constraint and the output torque constraint to obtain the maximum output torque under a given joint position and a given output torque direction.

[0130] In order to study the maximum output torque of a single joint in different output torque directions at a given position, the following Figure 11 The output torque can be decomposed into the orthogonal universal joint axis using the coordinate system of the universal joint. The output torque direction can be obtained from the universal joint torque:

[0131]

[0132] Among them, τ x , τ y are the components of the output torque on the universal joint shaft respectively.

[0133] Based on the rope tension constraint and the output torque constraint, the following optimization function can be constructed to find the maximum output torque for a given joint position and a given output torque direction:

[0134]

[0135]

[0136] Among them, f min , f max are the lower and upper limits of the rope tension respectively.

[0137] Taking into account the friction between the rope and the rope hole, the actual driving rope tension will be attenuated, and the actual driving joint tension cannot reach the upper and lower limits of tension. Define the tension variation coefficient:

[0138]

[0139] Among them, μ is the friction coefficient between the rope and the rope hole, α m is the rope envelope angle.

[0140] In addition, if Figure 7 As shown, in one embodiment, step S150 may include but is not limited to the following steps:

[0141] Step S710, assuming that each drive rope passes through the joint rope hole near the base end at the maximum envelope angle and assuming that static friction is used, obtain the maximum tension variation coefficient;

[0142] Step S720 : obtaining the actual driving pulling force according to the maximum pulling force variation coefficient after the friction force acts on it.

[0143] Since the tension attenuation coefficient is related to the rope envelope angle, and the angles of the joints near the base end cannot be determined, it is also impossible to determine the actual tension variation coefficient when the drive rope passes through. To obtain the driving tension, a conservative estimate is made here, assuming that each drive rope passes through the joint rope hole near the base end at the maximum envelope angle, that is, the maximum tension variation coefficient is taken, and static friction is also assumed. Therefore, the maximum tension variation coefficient is:

[0144]

[0145] Among them, α max is the maximum envelope angle of the rope.

[0146] Mapping matrix R i，i T Each element of reflects the rope tension arm and the direction of the torque generated by the tension. For the k-th driving rope, the joint torque it generates is:

[0147]

[0148] Among them, τ k is the joint torque component generated by the kth driving rope, (r 1k r 2k ) T is the mapping matrix R i，i T The k-th column element of .

[0149] We can determine whether the rope tension contributes positive work to the output torque by performing a dot product on the torque vector. Furthermore, we can see that in the minimum rope tension distribution algorithm, the maximum tension rope always contributes positive work, while the minimum tension rope always contributes negative work.

[0150] After the friction force acts, the actual driving force is:

[0151]

[0152] Where m represents the number of holes in the rope. For joint i, m = 2(i-1) + 1. It is the maximum value of the tension variation coefficient after m passes.

[0153] Since the direction of the friction force on the rope with the maximum tension is opposite to the driving tension, the rope tension gradually decays after passing through each rope guide hole after coming out of the drive box; the direction of the friction force on the rope with the minimum tension is in the same direction as the driving tension, so the rope tension increases step by step after passing through each rope guide hole; the tension of the other rope is between the two, and no matter what the direction of its friction force is, the final tension of the driving joint is also between the tensions of these two ropes.

[0154] In other words, due to the effect of friction, the rope tension range at the joint drive end is smaller than the allowed tension range set at the motor end:

[0155]

[0156] This means that the maximum output torque of the joint is also reduced. By modifying the upper and lower limits of the maximum output torque for a given joint position and a given output torque direction, a conservative calculation of the maximum output torque of the joint can be obtained while considering the influence of friction.

[0157] Step S160 , establishing an ideal mathematical model for the tension transmission process of the rope-driven joint, and analyzing the transient coupling effect under the rope spatial tension control.

[0158] In the inverse dynamics simulation of a rope-driven manipulator, the calculated desired driving force of the rope is directly input into the dynamic model. However, in a practical system, the rope tension control issue also needs to be considered.

[0159] The most straightforward control method for rope-driven joints is to use tension sensors to measure the tension in each rope and independently control the tension in each rope through a closed-loop control loop. However, research has shown that this joint control method can introduce transient coupling, resulting in coupled motion between different joints. This phenomenon is analyzed, and joint spatial control is proposed to eliminate this coupling.

[0160] To analyze the transient coupling phenomenon, we first simplify the tension transmission process of the rope-driven joint to establish an ideal mathematical model. The tension transmission process of the rope-driven joint is as follows: the motor drives the reduction gearbox and the lead screw, which in turn drives the slider to deform the taut rope, generating a change in tension that is ultimately transmitted to the rope-driven joint.

[0161] In addition, if Figure 8As shown, in one embodiment, step S160 may include but is not limited to the following steps:

[0162] Step S810: Ignore the friction in the transmission and transfer the mass of the transmission component to the slider, simplifying the model to a slider-driven rope model;

[0163] Step S820, making an assumption about the model of the rope-driven joint;

[0164] Step S830, obtaining the relationship between the output force of the driving motor and the movement of the slider according to the tension transmission model;

[0165] Step S840: According to the rope stiffness model, the rope tension is obtained by the position difference between the slider and the joint angle.

[0166] Ignoring the friction in the transmission, the mass of the transmission component can be transferred to the moving slider, which can be simplified to the model of the driving slider driving the rope as follows: Figure 12 shown.

[0167] The rope-driven joint has highly nonlinear characteristics. To analyze the transient response characteristics of the joint, the following assumptions are made for the rope-driven joint model:

[0168] The rope-joint velocity mapping matrix R is a constant. The velocity mapping matrix changes with the joint angle and is a nonlinear function of the joint angle. In transient analysis, this function can be linearized at the initial position. After linearization, R can be considered a constant.

[0169] The inertia of the connecting rod is much smaller than that of the slider. Because the mass of the designed robot arm connecting rod is small, its inertia is much smaller than the inertia of the drive mechanism, that is, much smaller than the inertia of the motor rotor, reducer, screw, slider, etc. At the same time, it is assumed that the arm moves freely and is not affected by external forces;

[0170] The relationship between rope tension and deformation is linear. To simplify the model, the nonlinear factors of rope tension-deformation are ignored and Hooke's law is used to describe the rope stiffness model.

[0171] Rope space tension control flow chart is as follows Figure 13 As shown in the figure, according to the tension transmission model, the output force of the drive motor and the movement of the slider have the following relationship:

[0172] s 2 M x X(s)=F motor (s)-F t (s),

[0173] Among them, X(s), F motor (s), F t (s) represent the Laplace transform of the slider position, motor output force and rope tension vector respectively.motor (s) = k i I(s),k i , I(s) are the current constant and the Laplace transform of the current respectively; M x is the equivalent mass matrix of the slider.

[0174] According to the rope stiffness model, the rope tension can be obtained by the position difference between the slider and the joint angle:

[0175] f t =k t (x-Rq),

[0176] Among them, k t is the rope stiffness coefficient matrix, and it is also assumed that all ropes have the same.

[0177] In addition, if Figure 9 As shown, in one embodiment, step S160 may include but is not limited to the following steps:

[0178] Step S910, using PD control in the rope tension closed-loop control to obtain the desired motor current;

[0179] Step S920, obtaining the relationship between the joint motion and the expected torque input;

[0180] Step S930: Analyze and determine the cause of the transient coupling phenomenon.

[0181] PD control can be used in the closed-loop control of rope tension to obtain the desired motor current:

[0182]

[0183] Among them, k p , k d are the proportional and differential coefficient matrices of PD control respectively.

[0184] Combining the linear relationship of the above formula, we can get:

[0185]

[0186] Multiply both sides of the equation by R. T , we can get:

[0187]

[0188] Where τ(s) is the actual joint output torque. Considering the dynamics in the joint space and ignoring the centrifugal force or Coriolis force, we have:

[0189] τ(s)=s 2 MQ(s)

[0190] The relationship between joint motion and desired torque input can be obtained:

[0191]

[0192] From the above formula, we can see that due to the existence of R in the denominator T The non-diagonal matrix of R causes coupling between the input desired torque and the joint motion, that is, the desired torque of one joint will act on other joints, resulting in transient coupling phenomenon.

[0193] The coupling phenomenon in joint motion is essentially caused by the discrepancy between the force and motion mappings between the rope drive and the joint. Directly closing the rope tension loop in rope space cannot eliminate this discrepancy. To eliminate transient coupling in joint motion, it is necessary to consider the relationship between force and motion mappings and implement compensatory decoupling in control.

[0194] Step S170: Perform joint motion analysis under joint space tension control.

[0195] In order to eliminate the transient coupling of the joint, the torque control of the rope-driven joint space is proposed. The flow chart of the joint space tension control is as follows: Figure 14 shown.

[0196] In addition, if Figure 10 As shown, in one embodiment, step S170 may include but is not limited to the following steps:

[0197] Step S1010, forming a joint torque control closed loop by comparing the expected and equivalent actual output torques;

[0198] Step S1020 , obtaining a driving current signal of the motor through joint and rope space conversion to weaken the joint motion coupling effect.

[0199] The actual rope tension is converted into an equivalent actual output torque. Therefore, the control closed loop of the joint torque can be formed by comparing the expected and equivalent actual output torques:

[0200]

[0201] in, is the equivalent joint control signal.

[0202] The motor drive current signal is obtained through joint-rope space transformation:

[0203]

[0204] Furthermore, combined with joint dynamics, we can get:

[0205]

[0206] From the above formula, we can see that since the joint inertia M is much smaller than the slider mass M x , in the quadratic term, the coupling term 1 / (R T R) is greatly weakened. For higher order terms, their coefficients are in the inverse of the stiffness coefficient 1 / k t The coupling effect of higher-order terms can be ignored due to the combined effect of the joint inertia M. Therefore, the tension control of the joint space can be replaced by the tension control of the rope space, which can effectively weaken the coupling effect of joint motion.

[0207] Step S180: establishing a target impedance model in the joint space.

[0208] The goal of impedance control is to track the impedance dynamics between the robot's target motion trajectory and the contact force, rather than simply tracking the trajectory. The dynamics of impedance can be described using a mass-damper-spring second-order system model, which consists of three parameters, namely the inertia coefficient M d , damping coefficient D d , stiffness coefficient K d By adjusting these three parameters, different impedance characteristics can be achieved, such as high rigidity, high damping, and high inertia. Impedance control can be intuitively understood as establishing a virtual impedance system between the actual and desired positions of the robot arm, and controlling the robot arm to track the force output of this virtual system. The "compliant" nature of the impedance system allows the robot arm to move compliantly.

[0209] In order to make the rope-driven manipulator joints compliant, a target impedance model can be established in the joint space:

[0210]

[0211] Where Δq is the error between the desired joint angle and the actual joint angle, is the error between the expected joint angular velocity and the actual joint angular velocity, is the error between the expected joint angular acceleration and the actual joint angular acceleration.

[0212] According to the dynamic model of the rope-driven arm, the expected joint torque output is:

[0213]

[0214] Since the acceleration of the joint cannot be measured directly, the control law cannot be used directly. The control output torque is simplified and the following is obtained: Figure 15 The joint impedance control flow chart shown is:

[0215]

[0216] Using the impedance control law, the target impedance model becomes:

[0217]

[0218] Compared to the ideal impedance model, the above equation cannot track the desired inertial characteristics. Furthermore, due to the small mass of the rope-driven manipulator, this impedance model is approximately a first-order system. Despite this, the rope-driven manipulator still possesses impedance characteristics of stiffness and damping, circumvents the difficulty of measuring joint angular acceleration, reduces system complexity, and enhances practicality.

[0219] In the description of this specification, if the reference terms "one embodiment," "some examples," "some embodiments," "illustrative embodiment," "example," "specific example," or "some examples" appear, it means that the specific features, structures, materials, or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present application. In this specification, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials, or characteristics described can be combined in any appropriate manner in any one or more embodiments or examples.

[0220] The above describes the implementation methods of the present application in detail in conjunction with the accompanying drawings, but the present application is not limited to the above implementation methods. Various changes can be made within the scope of knowledge possessed by ordinary technicians in the technical field without departing from the purpose of the present application.

Claims

1. An impedance control method, characterized in that: Perform dynamic modeling, organize the recursive form dynamic equations, and obtain the closed form dynamic equations in the joint space; According to the closed-form dynamic equation, the expression of the driving pulling force is obtained; According to the principle of virtual work and the mapping relationship between joints and rope lengths, the relationship between rope velocity vector, rope tension vector, joint velocity vector, and joint equivalent torque is obtained. The rope tension is calculated by the joint torque, and the driving tension solution can be expressed as the sum of the least squares solution and any null space solution; Analyze the joint torque output capacity under rope tension constraint; An ideal mathematical model for the tension transmission process of the rope-driven joint is established to analyze the transient coupling effect under the control of rope spatial tension. Conduct joint motion analysis under joint space tension control; Establish the target impedance model in the joint space.

2. The impedance control method according to claim 1, wherein: The kinetic modeling comprises: The force balance equation for the end link is established, and the equivalent driving torque of the universal joint is obtained by combining the direction of the joint axis defined by the DH coordinate system. The force balance equations for the other links are established, and the equivalent driving torque of the universal joint is obtained by combining the direction of the joint rotation axis defined by the DH coordinate system.

3. The impedance control method according to claim 1, wherein: The expression for obtaining the driving force includes: Find a feasible solution through the rope tension distribution algorithm; The scalar mapping relationship between the driving rope tension and the equivalent joint driving torque is obtained according to the rope and joint space mapping matrix.

4. The impedance control method according to claim 1, wherein: The driving force solution can be expressed as the sum of the least squares solution and any null space solution: The internal tension coefficient is selected. The internal tension is located in the right null space of the Jacobian matrix of the rope length function at the joint segment. The internal tension coefficient is selected arbitrarily.

5. The impedance control method according to claim 4, characterized in that: The selected internal tension coefficient includes: Find the minimum internal tension of the rope so that all rope tensions are greater than the lower limit of tension and obtain the minimum solution of the total tension; Calculate the internal tension coefficient that makes the tension of each driving rope exactly equal to the lower limit of tension, and take the maximum value among them; Make the minimum tension in the drive rope exactly equal to the lower limit of tension.

6. The impedance control method according to claim 1, wherein: The analysis of the joint torque output capacity under the rope tension constraint includes: Establish a coordinate system to decompose the output torque onto the orthogonal universal joint axes; The output torque direction is obtained from the universal joint torque; Based on the rope tension constraint and the output torque constraint, an optimization function can be constructed to obtain the maximum output torque under a given joint position and a given output torque direction.

7. The impedance control method according to claim 6, wherein: The analysis of the joint torque output capacity under the rope tension constraint further includes: Assuming that each driving rope passes through the joint rope hole near the base end at the maximum envelope angle and assuming that static friction is used, the maximum tension variation coefficient is obtained; According to the maximum pulling force variation coefficient, the actual driving pulling force is obtained after the friction force acts.

8. The impedance control method according to claim 1, wherein: An ideal mathematical model is established for the tension transmission process of the rope-driven joint to analyze the transient coupling effects under the control of rope spatial tension, including: Ignore the friction in the transmission, transfer the mass of the transmission component to the slider, and simplify the model to a slider driving the rope; Make assumptions about the model of the rope-driven joint; According to the tension transmission model, the relationship between the output force of the driving motor and the movement of the slider is obtained; According to the rope stiffness model, the rope tension is obtained by the position difference between the slider and the joint angle.

9. The impedance control method according to claim 8, wherein: Establishing an ideal mathematical model for the tension transmission process of the rope-driven joint and analyzing the transient coupling effect under the rope spatial tension control also include: Use PD control in the closed-loop control of rope tension to obtain the desired motor current; Get the relationship between joint motion and desired torque input; The reasons causing the transient coupling phenomenon are analyzed.

10. The impedance control method according to claim 1, wherein: The joint motion analysis under joint space tension control includes: By comparing the expected and equivalent actual output torques, a closed loop of joint torque control is formed; The driving current signal of the motor is obtained through spatial conversion of joints and ropes to weaken the coupling effect of joint motion.

Images