Constant force control method and system for multi-joint flexible mechanical arm

Abstract

The invention discloses a constant force control method and system for a multi-joint flexible mechanical arm, and belongs to the technical field of robots. The method specifically comprises the steps that through a force sensor installed at the tail end of the flexible mechanical arm, the difference value of contact force between the tail end of the flexible mechanical arm and the environment and expected force serves as a force error, and the difference value of the change rates of the contact force and the expected force serves as the force error change rate; calculating a damping coefficient and a corresponding stiffness coefficient variable quantity; an impedance control model is constructed and solved, and an expected trajectory of the flexible mechanical arm is obtained; obtaining a track deviation and a speed deviation; a fast terminal sliding mode controller is constructed, and a control law is solved in combination with an extended state observer; and carrying out driving control on the flexible mechanical arm by utilizing the obtained control law to realize constant force control on the tail end of the flexible mechanical arm. According to the invention, the response speed of the system and the stability of the transition process are ensured, the problem of impedance coefficient selection is solved, automatic adjustment can be carried out according to real-time information, and the control precision, the response speed and the anti-interference capability are improved.

Description

Technical Field

[0001] The present invention belongs to the field of robotics technology, and more specifically, relates to a constant force control method and system for a multi-joint flexible robotic arm. Background Art

[0002] There are many technologies for constant force control of flexible manipulators. The constant force control technology based on pneumatic principles uses the compressibility and elasticity of gas to achieve constant force output by adjusting the air pressure. It has a fast response speed and flexible buffering, but the air pressure control accuracy is low and is greatly affected by the stability of the air source. The constant force control technology based on the spring mechanism provides constant force through the elastic deformation of the spring. It has a simple structure, but the elastic coefficient is fixed, it is difficult to adjust the constant force in real time, and fatigue deformation is prone to occur. The constant force control technology based on force-position hybrid control combines force control and position control to give full play to the advantages of both, but the algorithm is complex, and the computing power and control accuracy requirements are high. The switching of force and position control requires precise judgment. The constant force control technology of flexible manipulators based on impedance control establishes a dynamic impedance relationship between the end of the manipulator and the environment, so that the manipulator adjusts the position and force output when subjected to external forces to achieve constant force control. It can better ensure the safety and reliability of the system, but faces the challenge of impedance parameter selection. At the same time, in the impedance control framework, which mainly includes the robot position control inner loop and force control outer loop, the commonly used inner loop position control generally has problems such as insufficient control accuracy, weak anti-interference ability and slow response speed. These problems not only directly affect the accuracy of position control, but also indirectly reduce the accuracy of constant force control at the end of the robot arm, because inaccurate position control makes it difficult to achieve accurate force control. Therefore, improving the accuracy and robustness of position control is crucial to improving the accuracy of constant force control at the end of the robot arm.

[0003] Prior art document 1 (CN116512242A) discloses a fuzzy variable impedance control method for a grinding and polishing robot based on a fast terminal sliding mode. Its shortcomings are that it only considers the influence of the damping coefficient on the control, and ignores the influence of the stiffness coefficient; it only uses fast terminal sliding mode control in the position inner loop, and when the disturbance is large, its control effect will be unsatisfactory. Summary of the invention

[0004] In order to solve the deficiencies in the prior art, the present invention provides a constant force control method and system for a multi-joint flexible robotic arm. The present invention uses impedance control to overcome the problems of low precision of pneumatic principle and great influence of air source, easy fatigue deformation of fixed elastic coefficient of spring mechanism, and complex and difficult switching of force-position hybrid control algorithm. A damping adjustment rate is added to ensure the response speed of the system and the smoothness of the transition process. The problem of impedance coefficient selection can be solved by changing the stiffness coefficient in the impedance controller using fuzzy control. The impedance parameters do not need to be accurately selected, and can be automatically adjusted according to real-time information, thereby enhancing the adaptability of the system and ensuring stable and accurate constant force control in different scenarios. In the position control of the flexible robotic arm, an extended state observer is added to estimate the total disturbance of the system, and the disturbance is compensated in the fast terminal sliding mode control, which can improve the control accuracy, response speed and anti-interference ability.

[0005] The present invention adopts the following technical solution.

[0006] A first aspect of the present invention provides a constant force control method for a multi-joint flexible robotic arm, comprising:

[0007] Through the force sensor installed at the end of the flexible robotic arm, the difference between the contact force between the end of the flexible robotic arm and the environment and the expected force is taken as the force error, and the difference in the rate of change between the two is taken as the rate of change of the force error;

[0008] According to different force errors and force error change rates, the damping coefficient and the corresponding stiffness coefficient change are calculated;

[0009] According to the calculated damping coefficient and the corresponding stiffness coefficient change, the impedance control model is constructed and solved to obtain the desired trajectory of the flexible manipulator;

[0010] According to the expected trajectory of the flexible manipulator and the actual trajectory of the flexible manipulator, the trajectory deviation and the speed deviation are obtained;

[0011] A fast terminal sliding mode controller is constructed based on trajectory deviation and speed deviation, and the control law is solved by combining the extended state observer.

[0012] The obtained control law is used to drive and control the flexible robotic arm to achieve constant force control at the end of the flexible robotic arm.

[0013] Preferably, the damping adjustment law of the damping coefficient B is expressed by the following formula:

[0014]

[0015] In the formula,

[0016] g 1 and g 2 are the proportionality coefficients of force error and force error change rate, respectively,

[0017] X d , X x are the reference position of the end of the flexible arm and the position of the flexible arm after adjustment,

[0018] E f is the force error,

[0019] E fc is the force error change rate.

[0020] Preferably, the fuzzy controller is based on different force errors E f And force error change rate E fc Calculate the corresponding change in stiffness coefficient, including:

[0021] Select the change of stiffness coefficient as the fuzzy output and the force error E f And force error change rate E fc As fuzzy input quantity;

[0022] Setting force error E f The domain range is [-N, N], the domain range of force error change rate is [-M, M], and the domain range of stiffness coefficient change is [-Z, Z]. According to the triangle membership, the force error E f , the force error change rate and the stiffness coefficient change are each divided into 7 fuzzy sets {PB, PM, PS, Z, NS, NM, NB}, among which PB represents positive large, PM represents positive middle, PS represents positive small, Z represents zero, NS represents negative small, NM represents negative middle, and NB represents negative large.

[0023] Preferably, the damping coefficient B is substituted into the impedance control model, the stiffness coefficient change ΔK is superimposed on the stiffness coefficient K in the impedance controller, and the adjusted position X of the end of the flexible arm is obtained by solving the impedance control model. s As the desired trajectory of the flexible manipulator, the impedance control model is expressed as follows:

[0024]

[0025] In the formula,

[0026] M, B, K are the inertia, damping and stiffness coefficients of the system respectively.

[0027] ΔK is the change in stiffness coefficient,

[0028] X d , are the reference position, velocity and acceleration of the end of the flexible arm,

[0029] X s , are the adjusted position, velocity and acceleration of the end of the flexible arm,

[0030] F.F d are the actual contact force and the expected force, respectively.

[0031] Preferably, according to the desired trajectory of the flexible robotic arm and the actual trajectory of the flexible robotic arm, the trajectory deviation and the speed deviation are obtained, which specifically include:

[0032] According to the dynamic equation of the actual motion of the robot arm, the state variables of each joint are calculated, including the bending angle ω and the rotation angle of the joint. Use state variables to construct an actual pose matrix, which represents the actual position and posture of the end of the robot arm;

[0033] According to the desired trajectory X of the flexible robotic arm s , the expected state variables of each joint are calculated through inverse kinematics solution, and an expected posture matrix is ​​constructed using the expected state variables. The expected posture matrix represents the expected position and posture of the end of the robot arm;

[0034] The difference between the expected pose and the actual pose is taken as the trajectory deviation e 1 , the difference between the first-order derivative of the expected posture and the actual posture is taken as the velocity deviation e 2 .

[0035] Preferably, a fast terminal sliding mode controller is constructed, which includes a sliding mode function, a sliding mode control strategy equation and a tracking error state equation. The result after the sliding mode function is derived is equal to the sliding mode control strategy equation to obtain an equation, and the tracking error state equation is substituted into the equation. Combined with the extended state observer equation, the control law u is obtained by solving it, which is expressed as the following formula:

[0036]

[0037] In the formula,

[0038] c 1 、c 2 is the proportional adjustment coefficient,

[0039] A=[M(q)-J m ], M(q) is the symmetric positive definite rigid body inertia matrix, J m is the positive definite motor inertia matrix,

[0040] is the Coriolis force matrix, is the first-order derivative of q,

[0041] e 2 is the speed deviation,

[0042] x 2d is the bending angular velocity of the node and the angular velocity The reference trajectory of

[0043] For x 2d The first derivative of

[0044] p,q are both odd numbers and 2>p / q>1,

[0045] is the total disturbance observed in the extended state observer,

[0046] v is the gain factor,

[0047] sgn(·) is the sign function.

[0048] Preferably, a fast terminal sliding mode controller is constructed, the fast terminal sliding mode controller includes a sliding mode function, a sliding mode control strategy equation and a tracking error state equation, and the sliding mode function is defined as:

[0049]

[0050] Define the sliding mode control strategy equation:

[0051]

[0052] In the formula,

[0053] e 1 is the trajectory deviation,

[0054] is the first-order derivative of the sliding mode function s.

[0055] Preferably, the tracking error state equation is expressed as follows:

[0056]

[0057] In the formula,

[0058] for e 1 The first derivative of

[0059] for e 2 The first derivative of .

[0060] Preferably, the total disturbance d suffered by the system is defined as the system expanded state variable x 3 , the total disturbance d of the system is estimated in the extended state observer, and the obtained The extended state observer equation is expressed as follows:

[0061]

[0062] In the formula,

[0063] L 1 , L 2 , L 3 is the observer gain,

[0064] are the observer state estimates, respectively, where

[0065] They are The first derivative of

[0066] x 1 =[qq m ] T , q is the vector of the motor rotation angle, q m is the vector of the motor's angular displacement.

[0067] The second aspect of the present invention provides a constant force control system of a multi-joint flexible robotic arm, which is used to run the constant force control method of a multi-joint flexible robotic arm described in the first aspect of the present invention, comprising:

[0068] Force error parameter solving module: used for taking the difference between the contact force and the expected force between the end of the flexible robotic arm and the environment as the force error through the force sensor installed at the end of the flexible robotic arm, and taking the difference between the change rates of the two as the force error change rate;

[0069] Impedance control parameter solution module: used to calculate the damping coefficient and the corresponding stiffness coefficient change according to different force errors and force error change rates;

[0070] Expected trajectory solving module: used to construct and solve the impedance control model according to the calculated damping coefficient and the corresponding stiffness coefficient change, and obtain the expected trajectory of the flexible manipulator;

[0071] Deviation solving module: used to obtain trajectory deviation and speed deviation according to the expected trajectory of the flexible robotic arm and the actual trajectory of the flexible robotic arm;

[0072] Control law solving module: used to build a fast terminal sliding mode controller based on trajectory deviation and speed deviation, and solve the control law in combination with the extended state observer;

[0073] Constant force control module: used to utilize the obtained control law to drive and control the flexible robotic arm, thereby achieving constant force control of the end of the flexible robotic arm.

[0074] Compared with the prior art, the beneficial effects of the present invention include at least:

[0075] 1. The use of impedance control has obvious advantages. It overcomes the problems of low precision of pneumatic principle and great influence of air source, easy fatigue deformation of fixed elastic coefficient of spring mechanism, and complex and difficult switching of force-position hybrid control algorithm. It brings high-precision control to meet the needs of precision machining; strong adaptability to complex environments; long-term stability without fatigue problems; strong integrability, and can be combined with other technologies to improve system performance and intelligence.

[0076] 2. The present invention adds a damping adjustment rate to ensure the system response speed and the smoothness of the transition process. The use of fuzzy control to change the stiffness coefficient in the impedance controller can solve the problem of impedance coefficient selection. It does not require precise selection of impedance parameters and can automatically adjust according to real-time information, thereby enhancing the adaptability of the system and ensuring stable and accurate constant force control in different scenarios. It can control the flexible robotic arm more smoothly and flexibly, thereby improving control accuracy. It simplifies design and debugging, and the algorithm is simple, making the process simpler and more efficient. It enhances robustness, maintains good control effects under adverse conditions, and improves reliability and stability.

[0077] Third, an extended state observer is added to the position control of the flexible manipulator to estimate the total disturbance of the system, and the disturbance is compensated in the fast terminal sliding mode control, which can improve the control accuracy, response speed and anti-interference ability. It can achieve high-precision control of the position of the manipulator, provide an accurate basis for constant force control, reduce the influence of position error, and has fast convergence characteristics, and can quickly respond to external interference and system changes. This brings many beneficial effects, including improving the accuracy of constant force control, reducing force fluctuations, enhancing the system's anti-interference ability, resisting external force interference and model errors, and improving the system's dynamic performance, so that it can better adapt to different work tasks and environmental changes. BRIEF DESCRIPTION OF THE DRAWINGS

[0078] Figure 1 It is a schematic diagram of the constant force control principle of the end of a flexible mechanical arm provided in accordance with an embodiment of the present invention;

[0079] Figure 2 It is a schematic diagram of fuzzy control rules provided according to an embodiment of the present invention. DETAILED DESCRIPTION

[0080] In order to make the purpose, technical scheme and advantages of the present invention clearer, the technical scheme of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. The described embodiments are only embodiments of a part of the present invention, not all embodiments. Based on the spirit of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work belong to the protection scope of the present invention.

[0081] like Figure 1As shown, embodiment 1 of the present invention provides a constant force control method for a multi-joint flexible robotic arm, comprising the following steps:

[0082] Step 1: Use the force sensor installed at the end of the flexible robotic arm to take the difference between the contact force between the end of the flexible robotic arm and the environment and the expected force as the force error E. f The difference between the two rates of change is taken as the force error change rate E fc .

[0083] Step 2: Calculate the damping coefficient B and the corresponding stiffness coefficient change ΔK according to the different force errors and force error change rates obtained in step 1.

[0084] The damping coefficient plays a key role in the control system, which affects the system's response speed and the smoothness of the transition process. If the damping coefficient is set improperly, it may cause the system to respond slowly or oscillate, thus affecting the stability of the entire system.

[0085] The damping coefficient B is adjusted according to the damping adjustment law. The damping adjustment law of the damping coefficient B is expressed by the following formula:

[0086]

[0087] In the formula,

[0088] g 1 and g 2 are the proportionality coefficients of force error and force error change rate, respectively,

[0089] X d , X s They are the reference position of the end of the flexible arm and the position of the flexible arm after adjustment.

[0090] In a preferred but non-limiting embodiment of the present invention, the stiffness coefficient of the flexible manipulator is processed by a fuzzy controller. Considering that humans will unconsciously adjust the stiffness of the arm according to the required contact force, the constant force control of the end of the flexible manipulator is achieved by simulating the human force control mechanism. Through the intelligent fuzzy control strategy, the flexible manipulator can automatically adjust its stiffness according to the required contact force, thereby achieving precise control of the constant force output of the end of the manipulator. Therefore, the change in stiffness coefficient is selected as the fuzzy output, and the force error and the force error change rate are selected as the fuzzy input.

[0091] Further preferably, the force error E is set fThe domain range is [-N, N], the domain range of force error change rate is [-M, M] and the domain range of stiffness coefficient change is [-Z, Z]. The force error, force error change rate and stiffness coefficient change are divided into 7 fuzzy sets respectively, which are marked by fuzzy language respectively. According to the triangle membership, they are divided into {PB, PM, PS, Z, NS, NM, NB}, where PB represents positive large, PM represents positive middle, PS represents positive small, Z represents zero, NS represents negative small, NM represents negative middle and NB represents negative large. The fuzzy control rules are as follows: Figure 2 Through this design, the fuzzy controller can calculate the corresponding stiffness coefficient change ΔK according to different force errors and force error change rates.

[0092] Step 3: According to the damping coefficient B and the change of stiffness coefficient ΔK calculated in step 3, the impedance control model is constructed and solved to obtain the desired trajectory.

[0093] The end of the flexible manipulator is equivalent to a second-order mass-damping-stiffness system. When the robot interacts with the environment, the ideal dynamic relationship between the position of the end of the flexible manipulator and the force is established by adjusting the parameters of the mass-damping-stiffness system.

[0094] In a preferred but non-limiting embodiment of the present invention, the impedance control model of the second-order mass-damping-stiffness system is:

[0095]

[0096] In the formula,

[0097] M, B, K are the inertia, damping and stiffness coefficients of the system respectively.

[0098] ΔK is the change in stiffness coefficient,

[0099] X d , are the reference position, velocity and acceleration of the end of the flexible arm,

[0100] X s , are the adjusted position, velocity and acceleration of the end of the flexible arm,

[0101] F.F d are the actual contact force and the expected force, respectively.

[0102] Further preferably, in step 3, the damping coefficient obtained in step 2 is substituted into the impedance control model, and the stiffness coefficient change ΔK obtained in step 2 is superimposed on the stiffness coefficient in the impedance controller, so that the impedance controller can adjust the damping coefficient and the stiffness coefficient according to the change of the force error and the force error change rate, so that the actual contact force F at the end of the flexible arm quickly reaches the desired force F d , the adjusted position X of the end of the flexible arm can be obtained by solving the differential equation of the impedance control model s as the expected trajectory.

[0103] Step 4: According to the expected trajectory obtained in step 3 and the actual trajectory of the flexible robot arm, the trajectory deviation and speed deviation are obtained.

[0104] In a preferred but non-limiting embodiment of the present invention, the state variables of each joint are first calculated according to the dynamic equation of the actual motion of the robot arm. These variables include the bending angle ω, the rotation angle ω of the joint, Then, these state variables are used to construct an actual pose matrix, which represents the actual position and posture of the end of the robotic arm. s , solve X by inverse kinematics s The expected state variables of each joint are calculated and used to construct an expected pose matrix, which represents the expected position and pose of the end of the robot. Finally, the difference between the expected pose and the actual pose is taken as the trajectory deviation e 1 The difference between the first-order derivative of the desired posture and the first-order derivative of the actual posture is taken as the velocity deviation e 2 .

[0105] Step 5: Construct a fast terminal sliding mode controller based on the trajectory deviation and speed deviation obtained in step 4, and solve the control law by combining the extended state observer;

[0106] The nonlinear dynamic equations of the actual trajectory of the multi-joint flexible robotic arm are:

[0107]

[0108] Combining the above equations, we get:

[0109]

[0110] In the formula,

[0111] M(q) represents the symmetric positive definite rigid body inertia matrix,

[0112] represents the Coriolis force matrix,

[0113] J mis the positive definite motor inertia matrix,

[0114] K represents the positive definite stiffness matrix of the flexible manipulator joint,

[0115] q represents the vector of the motor rotation angle,

[0116] q m The vector representing the angular displacement of the motor,

[0117] represents the first-order derivative of q,

[0118] represents the second-order derivative of q,

[0119] Indicates q m The second-order derivative of

[0120] u is the control law.

[0121] Let x 1 =[qq m ] T , the second derivative of the motor angular displacement vector The equation is rewritten as the state equation, the state equation, expressed as follows:

[0122]

[0123] In the formula,

[0124] For x 1 The first derivative of

[0125] For x 2 The first derivative of

[0126] A=[M(q) -J m ],

[0127]

[0128] At the same time, considering that the actual operation of the system contains uncertain disturbances such as unknown friction and external interference, let d be the total disturbance of the system and substitute it into the above formula (5) to obtain the state equation of the system under the disturbance:

[0129]

[0130] In order to improve the anti-disturbance ability of the system and estimate the disturbances to the system, an extended state observer is designed to observe the system state and disturbances in real time and compensate for the disturbances. The total disturbance d to the system is defined as the system extended state variable x 3 , x3 =d, then the state equation of the expansion state is expressed as follows:

[0131]

[0132] In the formula,

[0133] For x 3 The first derivative of

[0134] is the first-order derivative of d.

[0135] According to the state equation of the expanded state, the expanded state observer equation is constructed and expressed as follows:

[0136]

[0137] In the formula,

[0138] are the observer state estimates,

[0139] They are The first derivative of

[0140] For x 1 The first derivative of

[0141] L 1 , L 2 , L 3 is the observer gain, that is express The convergence of the observer error can be proved by using the second method of Lyapunov's law.

[0142] The tracking error equation of the flexible manipulator is defined as:

[0143]

[0144] In the formula,

[0145] x 1d is the bending angle ω and rotation angle of the joint The reference trajectory of

[0146] x 2d is the bending angular velocity of the node and the angular velocity The reference trajectory.

[0147] The tracking error state equation is obtained as:

[0148]

[0149] In the formula,

[0150] for e 1 The first derivative of

[0151] for e 2 The first derivative of .

[0152] is the total disturbance observed in the extended state observer, that is,

[0153] The sliding mode function is designed as:

[0154]

[0155] In the formula,

[0156] c 1 、c 2 is the proportional adjustment coefficient,

[0157] p and q are both odd numbers and 2>p / q>1.

[0158] The sliding mode control strategy equation is designed based on the following reaching law:

[0159]

[0160] In the formula,

[0161] ε is the gain factor,

[0162] sgn(·) is the sign function,

[0163] Represents the first-order derivative of the sliding mode function s.

[0164] Construct the state equation of the system under the perturbation;

[0165] Construct a fast terminal sliding mode controller, which includes a sliding mode function, a sliding mode control strategy equation and a tracking error state equation. According to the state equation of the system under the disturbance, the flexible manipulator tracking error equation is constructed, and based on the flexible manipulator tracking error equation, the tracking error state equation is constructed.

[0166] Construct the sliding mode function and sliding mode control strategy equation, let the result of sliding mode function derivation equal to sliding mode control strategy equation, substitute the tracking error state equation into the equation, combine the total disturbance estimate of the system obtained by the extended state observer equation, and solve the control law as follows:

[0167]

[0168] The present invention uses an extended state observer to estimate the total disturbance of the system, and compensates the estimated total disturbance in the controller, which can effectively eliminate the influence of the system disturbance on the control effect. Since the method of the present invention compensates for the system disturbance, it can effectively improve the system control accuracy and anti-interference ability compared with the traditional PID and the separate fast terminal sliding mode controller.

[0169] Step 6: Use the control law u obtained in step 5 to drive and control the flexible robotic arm to achieve constant force control at the end of the flexible robotic arm.

[0170] Then return to step 1, measure and subsequently calculate the end force, and repeat steps 1-6 to achieve constant force control of the end of the flexible robotic arm.

[0171] Compared with the prior art, the beneficial effects of the present invention include at least:

[0172] 1. The use of impedance control has obvious advantages. It overcomes the problems of low precision of pneumatic principle and great influence of air source, easy fatigue deformation of fixed elastic coefficient of spring mechanism, and complex and difficult switching of force-position hybrid control algorithm. It brings high-precision control to meet the needs of precision machining; strong adaptability to complex environments; long-term stability without fatigue problems; strong integrability, and can be combined with other technologies to improve system performance and intelligence.

[0173] 2. The present invention adds a damping adjustment rate to ensure the system response speed and the smoothness of the transition process. The use of fuzzy control to change the stiffness coefficient in the impedance controller can solve the problem of impedance coefficient selection. It does not require precise selection of impedance parameters and can automatically adjust according to real-time information, thereby enhancing the adaptability of the system and ensuring stable and accurate constant force control in different scenarios. It can control the flexible robotic arm more smoothly and flexibly, thereby improving control accuracy. It simplifies design and debugging, and the algorithm is simple, making the process simpler and more efficient. It enhances robustness, maintains good control effects under adverse conditions, and improves reliability and stability.

[0174] Third, an extended state observer is added to the position control of the flexible manipulator to estimate the total disturbance of the system, and the disturbance is compensated in the fast terminal sliding mode control, which can improve the control accuracy, response speed and anti-interference ability. It can achieve high-precision control of the position of the manipulator, provide an accurate basis for constant force control, reduce the influence of position error, and has fast convergence characteristics, and can quickly respond to external interference and system changes. This brings many beneficial effects, including improving the accuracy of constant force control, reducing force fluctuations, enhancing the system's anti-interference ability, resisting external force interference and model errors, and improving the system's dynamic performance, so that it can better adapt to different work tasks and environmental changes.

[0175] Embodiment 2 of the present invention provides a constant force control system for a multi-joint flexible robotic arm, and the constant force control method for a multi-joint flexible robotic arm described in embodiment 1 includes:

[0176] Force error parameter solving module: used for taking the difference between the contact force and the expected force between the end of the flexible robotic arm and the environment as the force error through the force sensor installed at the end of the flexible robotic arm, and taking the difference between the change rates of the two as the force error change rate;

[0177] Impedance control parameter solution module: used to calculate the damping coefficient and the corresponding stiffness coefficient change according to different force errors and force error change rates;

[0178] Expected trajectory solving module: used to construct and solve the impedance control model according to the calculated damping coefficient and the corresponding stiffness coefficient change, and obtain the expected trajectory of the flexible manipulator;

[0179] Deviation solving module: used to obtain trajectory deviation and speed deviation according to the expected trajectory of the flexible robotic arm and the actual trajectory of the flexible robotic arm;

[0180] Control law solving module: used to build a fast terminal sliding mode controller based on trajectory deviation and speed deviation, and solve the control law in combination with the extended state observer;

[0181] Constant force control module: used to utilize the obtained control law to drive and control the flexible robotic arm, thereby achieving constant force control of the end of the flexible robotic arm.

[0182] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the above embodiments, ordinary technicians in the relevant field should understand that the specific implementation methods of the present invention can still be modified or replaced by equivalents, and any modifications or equivalent replacements that do not depart from the spirit and scope of the present invention should be covered within the scope of protection of the claims of the present invention.

Claims

1. A constant force control method for a multi-joint flexible robotic arm, characterized in that: include: Through the force sensor installed at the end of the flexible robotic arm, the difference between the contact force between the end of the flexible robotic arm and the environment and the expected force is taken as the force error, and the difference in the rate of change between the two is taken as the rate of change of the force error; According to different force errors and force error change rates, the damping coefficient and the corresponding stiffness coefficient change are calculated; According to the calculated damping coefficient and the corresponding stiffness coefficient change, the impedance control model is constructed and solved to obtain the desired trajectory of the flexible manipulator; According to the expected trajectory of the flexible manipulator and the actual trajectory of the flexible manipulator, the trajectory deviation and the speed deviation are obtained; A fast terminal sliding mode controller is constructed based on trajectory deviation and speed deviation, and the control law is solved by combining the extended state observer. The obtained control law is used to drive and control the flexible robotic arm to achieve constant force control at the end of the flexible robotic arm.

2. The constant force control method of a multi-joint flexible robotic arm according to claim 1, characterized in that: The damping adjustment law of the damping coefficient B is expressed as follows: In the formula, g1 and g2 are the proportional coefficients of force error and force error change rate, respectively. X d , X s are the reference position of the end of the flexible arm and the position of the flexible arm after adjustment, E f is the force error, E fc is the force error change rate.

3. The constant force control method of a multi-joint flexible robotic arm according to claim 1, characterized in that: The fuzzy controller is based on different force errors E f And force error change rate E fc Calculate the corresponding change in stiffness coefficient, including: Select the change of stiffness coefficient as the fuzzy output and the force error E f And force error change rate E fc As fuzzy input quantity; Setting force error E f The domain range is [-N, N], the domain range of force error change rate is [-M, M], and the domain range of stiffness coefficient change is [-Z, Z]. According to the triangle membership, the force error E f , the force error change rate and the stiffness coefficient change are each divided into 7 fuzzy sets {PB, PM, PS, Z, NS, NM, NB}, among which PB represents positive large, PM represents positive middle, PS represents positive small, Z represents zero, NS represents negative small, NM represents negative middle, and NB represents negative large.

4. The constant force control method of a multi-joint flexible robotic arm according to claim 1, characterized in that: Substitute the damping coefficient B into the impedance control model, superimpose the stiffness coefficient change ΔK on the stiffness coefficient K in the impedance controller, and obtain the adjusted position X of the end of the flexible arm by solving the impedance control model. s As the desired trajectory of the flexible manipulator, the impedance control model is expressed as follows: In the formula, M, B, K are the inertia, damping and stiffness coefficients of the system respectively. ΔK is the change in stiffness coefficient, X d , are the reference position, velocity and acceleration of the end of the flexible arm, X s , are the adjusted position, velocity and acceleration of the end of the flexible arm, F.F d are the actual contact force and the expected force, respectively.

5. The constant force control method of a multi-joint flexible robotic arm according to claim 1, characterized in that: According to the expected trajectory of the flexible manipulator and the actual trajectory of the flexible manipulator, the trajectory deviation and speed deviation are obtained, including: According to the dynamic equation of the actual motion of the robot arm, the state variables of each joint are calculated, including the bending angle ω and the rotation angle of the joint. Use state variables to construct an actual pose matrix, which represents the actual position and posture of the end of the robot arm; According to the desired trajectory X of the flexible robotic arm s , the expected state variables of each joint are calculated through inverse kinematics solution, and an expected posture matrix is ​​constructed using the expected state variables. The expected posture matrix represents the expected position and posture of the end of the robot arm; The difference between the expected posture and the actual posture is taken as the trajectory deviation e1, and the difference between the first-order derivative of the expected posture and the actual posture is taken as the velocity deviation e2.

6. The constant force control method of a multi-joint flexible robotic arm according to claim 1, characterized in that: Construct a fast terminal sliding mode controller. The fast terminal sliding mode controller includes a sliding mode function, a sliding mode control strategy equation, and a tracking error state equation. Let the result of the sliding mode function derivative be equal to the sliding mode control strategy equation to obtain an equation. Substitute the tracking error state equation into the equation and combine it with the extended state observer equation to solve the control law u, which is expressed as the following formula: In the formula, c1 and c2 are proportional adjustment coefficients, A=[M(q) -J m ], M(q) is the symmetric positive definite rigid body inertia matrix, J m is the positive definite motor inertia matrix, is the Coriolis force matrix, is the first-order derivative of q, e2 is the speed deviation, x 2d is the bending angular velocity of the node and the angular velocity The reference trajectory of For x 2d The first derivative of p,q are both odd numbers and 2>p / q>1, is the total disturbance observed in the extended state observer, ε is the gain factor, sgn(·) is the sign function.

7. The constant force control method of a multi-joint flexible robotic arm according to claim 6, characterized in that: Construct a fast terminal sliding mode controller. The fast terminal sliding mode controller includes a sliding mode function, a sliding mode control strategy equation, and a tracking error state equation. The sliding mode function is defined as: Define the sliding mode control strategy equation: In the formula, e1 is the trajectory deviation, is the first-order derivative of the sliding mode function s.

8. The constant force control method of a multi-joint flexible robotic arm according to claim 7, characterized in that: The tracking error state equation is expressed as follows: In the formula, is the first-order derivative of e1, is the first-order derivative of e2.

9. The constant force control method of a multi-joint flexible robotic arm according to claim 8, characterized in that: The total disturbance d of the system is defined as the system extended state variable x3, and the total disturbance d of the system is estimated in the extended state observer to obtain The extended state observer equation is expressed as follows: In the formula, L1, L2, L3 are observer gains, are the observer state estimates, respectively, where They are The first derivative of x1=[qq m ] T , q is the vector of the motor rotation angle, q m is the vector of the motor's angular displacement.

10. A constant force control system for a multi-joint flexible robotic arm, used to run a constant force control method for a multi-joint flexible robotic arm according to any one of claims 1 to 9, characterized in that: Force error parameter solving module: used for taking the difference between the contact force and the expected force between the end of the flexible robotic arm and the environment as the force error through the force sensor installed at the end of the flexible robotic arm, and taking the difference between the change rates of the two as the force error change rate; Impedance control parameter solution module: used to calculate the damping coefficient and the corresponding stiffness coefficient change according to different force errors and force error change rates; Expected trajectory solving module: used to construct and solve the impedance control model according to the calculated damping coefficient and the corresponding stiffness coefficient change, and obtain the expected trajectory of the flexible manipulator; Deviation solving module: used to obtain trajectory deviation and speed deviation according to the expected trajectory of the flexible robotic arm and the actual trajectory of the flexible robotic arm; Control law solving module: used to build a fast terminal sliding mode controller based on trajectory deviation and speed deviation, and solve the control law in combination with the extended state observer; Constant force control module: used to utilize the obtained control law to drive and control the flexible robotic arm, thereby achieving constant force control of the end of the flexible robotic arm.

Images