CN109108954B - Torque control system and torque control method of power joint device - Google Patents
Torque control system and torque control method of power joint device Download PDFInfo
- Publication number
- CN109108954B CN109108954B CN201710481780.XA CN201710481780A CN109108954B CN 109108954 B CN109108954 B CN 109108954B CN 201710481780 A CN201710481780 A CN 201710481780A CN 109108954 B CN109108954 B CN 109108954B
- Authority
- CN
- China
- Prior art keywords
- torque
- value
- motor
- hat
- derivative
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J13/00—Controls for manipulators
- B25J13/08—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices
- B25J13/085—Force or torque sensors
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/10—Programme-controlled manipulators characterised by positioning means for manipulator elements
- B25J9/12—Programme-controlled manipulators characterised by positioning means for manipulator elements electric
- B25J9/126—Rotary actuators
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1628—Programme controls characterised by the control loop
- B25J9/1633—Programme controls characterised by the control loop compliant, force, torque control, e.g. combined with position control
Abstract
The invention relates to a torque control system of a power joint device and a torque control method thereof, wherein the system comprises a signal acquisition unit and a signal processing unit, wherein the signal acquisition unit comprises a rotary encoder, a torque sensor, an angle sensor and a current sensor; the method comprises the steps that a rotary encoder collects the rotation angle theta m of a motor, a torque sensor collects the relative torque Ts between an upper arm and a lower arm, an angle sensor collects the relative rotation angle theta l between the upper arm and the lower arm, and a current sensor collects the current Ifbk of the motor during working; the signal processing unit comprises a memory and a processor, the processor comprises a torque loop controller and a current loop controller, and the output of the torque loop controller is a control target Tm of the current loop controller; and Tm ═ a (de-f (s)) + B ═ T _ hat + Z _ hat. The invention achieves the torque control with quick response and strong robustness under the condition that the robot joint has flexibility and the friction force is changed quickly.
Description
Technical Field
The invention relates to a power joint device, in particular to a torque control system of the power joint device and a torque control method thereof.
Background
Robots are the common name for automatic control machines (Robot) that include all machines that simulate human behavior or thought and other creatures (e.g., machine dogs, machine cats, etc.). The existing robot field widely applies power joints which can be used for simulating the joint extension or bending of a human body, wherein, an exoskeleton robot wearable by the human body generally has a plurality of power joints, each power joint has an upper arm, a lower arm, a motor and a speed reducer, the upper arm and the lower arm can be extended or bent under the drive of the motor and the speed reducer, the power joints also generally have a plurality of sensors, including a joint torque sensor, an upper arm angle sensor, a lower arm angle sensor, a motor rotary encoder and a motor current sensor, for acquiring corresponding data and feeding the data back to a controller as the basis data for adjustment, such as a whole body exoskeleton assistant robot for carrying disclosed in chinese patent 201611165523.7, the robot mentioned in the patent is also provided with sensors such as the upper arm angle sensor and the lower arm angle sensor, and the angle sensor is used for measuring the angle value at the joint, and controlling the corresponding joint to move by the central control device according to the measured angle value.
Because the joints of the robot are made by simulating the joints of the human body and are certainly different from the joints of the human body in the aspect of flexibility, a force sensor needs to be embedded into a power joint of the robot, so that flexibility is introduced into a joint system of the robot, the joint transmission of the robot generally adopts a harmonic speed reducer to reduce speed, and the flexibility is further brought to the joint system, so that the joints have flexibility characteristics and can absorb shock and reduce impact, but the adoption of the harmonic speed reducer brings obvious lag, so that the control difficulty is increased, the effect of adopting classical PID control is not good, for example, Chinese patent No. 201494011.X provides a flexible driving unit control method for the joints of the robot with tension and joint position feedback, the method mainly realizes tension feedback and joint full-closed-loop control of a flexible driving device for the joints of the robot, so as to reduce the control error of the flexible driving joints, the method comprises the steps of improving system frequency response, specifically designing feedforward control according to an elastic deformation formula, designing a feedback controller through motor angle estimation, and realizing control by adopting a variable coefficient PID control method. However, the control method mentioned in the above patent is still improved by the conventional PID, and the control response speed of the whole joint system is not good enough. In addition, chinese patent 201510712869.3 provides a method for controlling a wearable robot, which is as follows: when the angle of the robot joint exceeds a predetermined allowable angle range R, the angle of the robot joint is returned to within the allowable angle range R, and when the robot joint is controlled so that the angle of the robot joint is returned to within the allowable angle range R and the angle of the robot joint reaches a predetermined angle, a torque is generated in the robot joint so that the angle of the robot joint is maintained at a predetermined intersection for a predetermined period of time, that is, the joint angle is controlled to be within a certain range, and the mentioned control embodiment is PID control or state feedback control, but the embodiment of the control is not disclosed.
In the prior art, besides traditional PID control, modern control theory includes variable structure control, state feedback control and the like, the methods are faster in response speed and strong in robustness than the traditional PID control, but all the methods need to obtain state quantities of a system, the state quantities are generally difficult to directly measure and obtain, observation or calculation and estimation are needed, control errors are often caused, even control is unstable, in addition, a power joint generally has friction force during movement, the friction force belongs to fast change interference, and the accurate control is also difficult.
Therefore, it is necessary to design a torque control system of a power joint device to achieve torque control with fast response and strong robustness under the condition that the robot joint has flexibility and friction force changes rapidly, so that the related system of the robot or the exoskeleton with the power joint device has high sensitivity and stability.
Disclosure of Invention
The invention aims to overcome the defects of the prior art and provides a torque control system of a power joint device and a torque control method thereof.
In order to achieve the purpose, the invention adopts the following technical scheme: a torque control system of a power joint device comprises a signal acquisition unit and a signal processing unit, wherein the signal acquisition unit comprises a rotary encoder connected with a motor for driving a joint to move, a torque sensor for measuring the relative torque between an upper arm and a lower arm, an angle sensor between the upper arm and the lower arm of the joint and a current sensor connected with the motor for driving the joint to move; the motor control system comprises a rotary encoder, a torque sensor, an angle sensor and a current sensor, wherein the rotary encoder acquires a rotation angle theta m of the motor, the torque sensor acquires a relative torque Ts between an upper arm and a lower arm, the angle sensor acquires a relative rotation angle theta l between the upper arm and the lower arm, and the current sensor acquires a current Ifbk when the motor works; the signal processing unit comprises a memory and a processor, a torque loop controller and a current loop controller are arranged in the processor, and the output of the torque loop controller is a control target Tm of the current loop controller;
and Tm ═ a (de-f (s)) + B ═ T _ hat + Z _ hat;
a, B is a constant coefficient determined according to the system model; de is an error derivative value, the error e is equal to Tref-Ts or filtered data thereof, and Tref is a control target of the moment ring controller; f (S) is a synovial membrane approximation law function, wherein S ═ Cs × de + e, and Cs is an adjustable coefficient; t _ hat is Ts or data after filtering processing; z _ hat is system disturbance data, the system disturbance data Z _ hat includes a motor frictional disturbance value Z13 and/or a load and frictional disturbance value Z23 of the upper arm and the lower arm, the system disturbance data Z _ hat is an optional item, and the system disturbance data Z _ hat satisfies C × Z13 or D × Z23 or the sum of C × Z1 and D × Z23, C ═ Jm, D ═ N ═ Jm, Jm is the rotational inertia of the motor, and N is the reduction gear ratio of the power joint.
The further technical scheme is as follows: the constant coefficient A, B satisfies:
A＝N*Jm/Cs；
B＝1/N+N*Jm/Jl；
wherein Jl is the moment of inertia of the power joint and the load, and Cs is an adjustable coefficient.
The further technical scheme is as follows: and a processing module, a differential filter and an extended observer are arranged in the torque loop controller, an error derivative value de is obtained through observation of the differential filter or the extended observer, system interference data Z _ hat is obtained through the extended observer, and data obtained by the differential filter and the extended observer are input to the processing module to be processed and output to a control target Tm of the current loop controller.
The further technical scheme is as follows: the extended observer comprises a first extended observer and at least one second extended observer, the first extended observer observes a rotation angle thetam of the motor or a filter value of the rotation angle thetam and outputs a motor friction interference value z13, and the second extended observer outputs a load of an upper arm and a lower arm and a friction interference value z 23; wherein the first extended observer satisfies the following condition:
eq11：e1＝z11-θm_hat；
eq12：dz11＝z12-β10*e1；
eq13：dz12＝z13-β11*fal(e1,α11,11)+b11*Ifbk_hat+b12*Ts_hat；
eq14：dz13＝-β12*fal(e1,α12,12)；
wherein dz11, dz12 and dz13 are respectively derivatives of z11, z12 and z13, β 10, β 11, β 12, α 11, α 12, 11 and 12 are adjustable parameters, b11 and b12 are constants set according to the measured system parameters, one of the constants is b11 ═ 1/Jm, b12 ═ 1/(Jm N), and fal is a monotonic function;
the input variables of the first extended observer are thetam _ hat, Ifbk _ hat and Ts _ hat, wherein thetam _ hat is a rotating angle thetam of the motor collected by a rotary encoder or a filter value thereof, Ifbk _ hat is a current Ifbk of the motor collected by a current sensor when the motor works or a filter value thereof, Ts _ hat is a relative torque Ts between an upper arm and a lower arm collected by a torque sensor or a filter value thereof, and the output variables are z11, z12 and z13, wherein z11 approaches the rotating angle thetam of the motor, z12 approaches a derivative value of the rotating angle thetam of the motor, and z13 approaches a current motor friction interference value;
the second said extended observer satisfies the following condition:
eq21：e2＝z21-θl_hat；
eq22：dz21＝z22-β20*e2；
eq23：dz22＝z23-β21*fal(e2,α21,21)+b22*Ts_hat；
eq24：dz23＝-β22*fal(e2,α22,22)；
the dz21, dz22 and dz23 are respectively corresponding to derivatives of z21, z22 and z23, β 20, β 21, β 22, α 21, α 22, 21 and 22 are adjustable parameters, b22 is a constant set according to a measured system parameter, wherein one of the constants is b22 ═ 1/Jl, Jl is the rotational inertia of the power joint and the load, and fal is a monotonic function;
the input variables of the second extended observer are theta l _ hat and Ts _ hat, theta l _ hat is a relative angle theta l between an upper arm and a lower arm collected by an angle sensor or a filter value of the relative angle theta l, Ts _ hat is a relative torque Ts between an upper arm and a lower arm collected by a torque sensor or a filter value of the relative torque Ts, and the output variables are z21, z22 and z23, wherein z21 approaches the relative angle theta l between the upper arm and the lower arm, z22 approaches a derivative value of the relative angle theta l between the upper arm and the lower arm, and z23 approaches a load and friction interference value of the upper arm and the lower arm.
The further technical scheme is as follows: the error derivative value de satisfies de ═ dTRef-dTs, wherein dTs ═ z32, and dTRef ═ dz 41;
the extended observer includes a third extended observer, wherein the third extended observer outputs z32, the third extended observer satisfying the following condition:
eq31：e3＝z31-Ts_hat；
eq32：dz31＝z32-β30*e3；
eq33：dz32＝z33-β31*fal(e3,α31,31)+b31*Ifbk_hat-b32*Ts_hat；
or eq33b dz32 ═ z33- β 31 ═ fal (e3, α 31,31) + b31 × Ifbk-b32 × + b33 × z13+ b34 × z 23;
eq34：dz33＝-β32*fal(e3,α32,32)；
wherein dz31, dz32 and dz33 are respectively corresponding to derivatives of z31, z32 and z33, β 30, β 31, β 32, α 31, α 32, 31 and 32 are adjustable parameters, b31 and b32 are constants set according to measured system parameters, one of the constants is b31 ═ K/(N Jm), b32 ═ K/Jl + K/(N ═ N Jm), K is an equivalent spring stiffness coefficient from an output end of the motor to a joint load, Jl is a dynamic joint and load rotational inertia, fal is a monotonic function, b33 and b34 are constants set according to the measured system parameters, one of the constants is b33 ═ K/N, and b34 ═ K;
the third extended observer input variables are Ifbk _ hat, Ts _ hat, Ifbk _ hat, which is the current Ifbk or a filtered value thereof during operation of the motor, Ts _ hat is the relative torque Ts between the upper and lower arms or a filtered value thereof, and the output variables are z31, z32, z33, where z31 approaches Ts and z32 approaches the derivative value of Ts.
The further technical scheme is as follows: the derivative filter comprises a first derivative filter, wherein the first derivative filter output z 41; and the first differential filter satisfies the following condition:
eq41：e4＝z41-Tref；
eq42：dz41＝-β40*fal(e4,α42,42)；
wherein dz41 is a derivative of z41, β 40, α 42, 42 are adjustable parameters, fal is a monotonic function, the input variable of the first differential filter is a control target Tref of the torque loop controller or a filter value thereof, the output variable is z41 and dz41, wherein z41 approaches a control target Tref value of the torque loop controller, and dz41 approaches a derivative value dTref of the control target Tref of the torque loop controller.
The further technical scheme is as follows: the differential filters further comprise a second differential filter, a third differential filter, a fourth differential filter, one or more fifth differential filters; the second differential filter, the third differential filter, the fourth differential filter and the fifth differential filter respectively perform filtering and differential processing on at least one of a relative torque Ts between the upper arm and the lower arm, a rotation angle thetam of the motor, a relative angle thetal between the upper arm and the lower arm, and a current Ifbk when the motor works; wherein the second derivative filter satisfies the following condition:
eq51：e5＝z51-Ts；
eq52：dz51＝-β50*fal(e5,α52,52)；
wherein dz51 is a derivative of z51, β 50, α 52, 52 are adjustable parameters, fal is a monotonic function, the input variable is the relative torque Ts between the upper and lower arms or a filtered value thereof, the output variables are z51, dz51, wherein z51 approaches the value of the relative torque Ts between the upper and lower arms, and dz51 approaches the derivative value dTs of the relative torque Ts between the upper and lower arms;
the third derivative filter satisfies the following condition:
eq61：e6＝z61-θm；
eq62：dz61＝-β60*fal(e6,α62,62)；
wherein dz61 is a derivative of z61, β 60, α 62, 62 are adjustable parameters, fal is a monotonic function, an input variable is a rotation angle theta m of the motor or a filter value thereof, output variables are z61 and dz61, z61 approaches a value of the rotation angle theta m of the motor, and dz61 approaches a derivative value d theta m of the rotation angle theta m of the motor;
the fourth differential filter satisfies the following condition:
eq71：e7＝z71-θl；
eq72：dz71＝-β70*fal(e7,α72,72)；
wherein dz71 is a derivative of z71, β 70, α 72, 72 are adjustable parameters, fal is a monotonic function, an input variable is a relative angle theta l of rotation between the upper arm and the lower arm or a filtered value thereof, an output variable is z71, dz71, wherein z71 approaches a numerical value of the relative angle theta l of rotation between the upper arm and the lower arm, and dz71 approaches a derivative value d theta l of the relative angle theta l of rotation between the upper arm and the lower arm;
the fifth differential filter satisfies the following condition:
eq81：e8＝z81-Ifbk；
eq82：dz81＝-β80*fal(e8,α82,82)；
where dz81 is the derivative of z81, β 80, α 82, 82 are adjustable parameters, fal is a monotonic function, the input variable is the current Ifbk during motor operation or a filtered value thereof, and the output variables are z81, dz81, where z81 approximates the value of the current Ifbk during motor operation and dz81 approximates the derivative value dlfbk of the current Ifbk during motor operation.
The further technical scheme is as follows: the sliding mode approximation law function F (S) is a constant speed approximation law function F ═ sign (S), or an exponential approximation law function F ═ ks ^ S-sign (S)), or a power approximation law function F ═ ks ^ abs (S)), wherein ks and alpha are adjustable parameters, sign is a sign function, and abs is an absolute value function.
The further technical scheme is as follows: the monotonic function fal function is a power function f1 or an arc tangent nonlinear function f 2;
wherein the power function f1 is expressed as:
the arctangent nonlinear function f2 is expressed as f2(e, α) β atan (2 α e/pi).
The invention also provides a torque control method of the torque control system of the power joint device, which comprises the following steps:
acquiring a rotation angle theta m of the motor, a relative torque Ts between an upper arm and a lower arm, a relative rotation angle theta l between the upper arm and the lower arm, a current Ifbk when the motor works and a control target Tref of a torque loop controller;
observing or filtering the thetam, thetal and Ts to obtain an output z13 of the first extended observer, an output z23 of the second extended observer, an output z32 of the third extended observer and an output dz41 of the first differential filter;
according to e-Tref-Ts, wherein e is the deviation or the approximate value of the actual measurement value Ts and the control target Tref; de-dz 41-z32 or de-z 32, where de is an approximation of the derivative of the deviation e; s ═ Cs × de + e, where S is a synovial surface parameter and Cs is an adjustable coefficient; tm ═ a (de-f (s)) + B × + Ts + C × z13+ D × z23, where Tm is the torque loop controller output value; obtaining an output Tm of a torque loop controller;
and the current loop controller takes Tm as a control target, controls the output control torque of the motor to approach Tm, and returns to the step of acquiring the rotation angle theta m of the motor, the relative torque Ts between the upper arm and the lower arm, the relative rotation angle theta l between the upper arm and the lower arm, the current Ifbk when the motor works and the control target Tref of the torque loop controller.
The further technical scheme is as follows: the step of observing or filtering θ m, θ l, Ts to obtain the output z13 of the first extended observer, the output z23 of the second extended observer, the output z32 of the third extended observer, and the output dz41 of the first derivative filter further comprises:
the input Tref, Ts, θ m, θ l, Ifbk are observed respectively to obtain output data z41, dz41, z51, dz51, z61, z71, z 81.
Compared with the prior art, the invention has the beneficial effects that: the torque control system of the power joint device of the invention is characterized in that a rotary encoder, an angle sensor, a torque sensor and a current sensor are arranged on the power joint device, synovial membrane control is adopted in a signal processing unit to realize quick response, an extended observer is adopted to observe system interference and obtain high-quality derivatives of related observed quantities, a differential filter is adopted to filter input data and obtain derivatives, a processing module is utilized to process a control target of an output current loop controller to control the output torque of a joint, by performing filtering, differentiation, observation and control calculation on the input data and the control target data, so as to achieve the torque control with quick response and strong robustness under the condition that the robot joint has flexibility and the friction force is changed quickly, the related system of the robot or the exoskeleton with the power joint device has high sensitivity and stability.
The invention is further described below with reference to the accompanying drawings and specific embodiments.
Drawings
FIG. 1 is a sectional view of a power joint device according to an embodiment of the present invention;
FIG. 2 is a schematic diagram illustrating the operation of a torque control system of a power joint apparatus according to an embodiment of the present invention;
FIG. 3 is a block diagram of a torque control system of a powered joint assembly according to an embodiment of the present invention;
fig. 4 is a flowchart of a torque control method of a torque control system of a power joint device according to an embodiment of the present invention.
Detailed Description
In order to more fully understand the technical content of the present invention, the technical solution of the present invention will be further described and illustrated with reference to the following specific embodiments, but not limited thereto.
As shown in specific embodiments of fig. 1 to 4, the torque control system of the power joint device provided in this embodiment can be applied to the field of robots, is particularly suitable for exoskeleton robots, and achieves torque control with fast response and strong robustness under the condition that the robot joint 1 has flexibility and rapid change of friction force, so that the robot or the exoskeleton related system having the power joint 1 device has high sensitivity and stability.
The torque control system of the power joint device provided by the embodiment comprises a signal acquisition unit and a signal processing unit, wherein the signal acquisition unit comprises a rotary encoder 16 connected with a motor 13 for driving the joint 1 to move, a torque sensor 15 for measuring the relative torque between an upper arm 11 and a lower arm 12, an angle sensor 17 positioned between the upper arm 11 and the lower arm 12 of the joint 1 and a current sensor 21 connected with the motor 13 for driving the joint 1 to move; wherein, the rotary encoder 16 acquires a rotation angle θ m of the motor 13, the torque sensor 15 acquires a relative torque Ts between the upper arm 11 and the lower arm 12, the angle sensor 17 acquires a relative rotation angle θ l between the upper arm 11 and the lower arm 12, and the current sensor 21 acquires a current Ifbk when the motor 13 works; the signal processing unit comprises a memory 23 and a processor 22, a torque loop controller 24 and a current loop controller 25 are arranged in the processor 22, and the output of the torque loop controller 24 is a control target Tm of the current loop controller 25;
and Tm ═ a (de-f (s)) + B ═ T _ hat + Z _ hat;
a, B is a constant coefficient determined according to the system model; de is an error derivative value, the error e is equal to Tref-Ts or filtered data thereof, and Tref is a control target of the torque loop controller 24; f (S) is a synovial membrane approximation law function, wherein S ═ Cs × de + e, and Cs is an adjustable coefficient; t _ hat is Ts or data after filtering processing; the system disturbance data Z _ hat includes a frictional disturbance value Z13 of the motor 13 and/or a load and frictional disturbance value Z23 of the upper arm 11 and the lower arm 12, and satisfies C × Z13 or D × Z23 or a sum of C × Z1 and D × Z23, C ═ Jm, D ═ N × Jm, Jm is a rotational inertia of the motor 13, and N is a reduction gear ratio of the reduction gear 14 of the power joint 1.
The signal acquisition unit is mainly arranged on a power joint 1 device, wherein, referring to fig. 1, the power joint 1 device comprises an upper arm 11, a lower arm 12, a motor 13 and a speed reducer 14, the upper arm 11 and the lower arm 12 can rotate relatively under the action of the motor 13 and the speed reducer 14, and a relative angle and a relative torque exist between the upper arm 11 and the lower arm 12 in the process of relative rotation, so that a torque sensor 15 is adopted to measure the relative torque Ts between the upper arm 11 and the lower arm 12, an angle sensor 17 is adopted to acquire the relative angle θ l of rotation between the upper arm 11 and the lower arm 12, in addition, for the motor 13, a motor 13 driving circuit is arranged on the power joint 1 device and is used for driving the motor 13 to rotate, because friction exists inside the motor 13, the current output by the motor 13 driving circuit is larger than the current when the motor 13 works, a current sensor 21 is used for acquiring the current Ifbk when the motor, i.e., the actual current of the motor 13, as one of the data for the subsequent torque loop controller 24 to compensate.
The processor 22 is provided with instructions for implementing various filtering and observation, and can send out control instructions to control the motor 13 to output torque according to the requirements of the instructions, generally, programs corresponding to the control instructions are stored in the memory 23, and in actual application, the processor 22 calls the programs in the memory 23 in real time and executes a signal processing process.
For the above mentioned Tm ═ a (de-f (s)) + B ═ T _ hat + Z _ hat, and refer to fig. 2.
Within a set time, a certain current is supplied to a motor 13 of the power joint 1 device to enable the power joint 1 device to start to operate, corresponding data is collected by a rotary encoder 16, a torque sensor 15, an angle sensor 17 and a current sensor 21, a target Tref is controlled by a binding force torque loop controller 24, namely, a torque value theoretically input by the torque loop controller 24, the data and the control target Tref are input into the torque loop controller 24 to be filtered, differentiated, observed and controlled, a control target Tm of a current loop controller 25, namely, an input torque value theoretically required by the current loop controller 25 is calculated, the control target Tm of the current loop controller 25 and the data collected by the current sensor 21 pass through the current loop controller 25 to output a current capable of enabling the power joint 1 device to reach high sensitivity and are input into the motor 13, a torque approaching Tm is output by the motor 13, to drive the powered joint 1 device to give a response.
In other words, the workflow of the entire system is: the actual working data of the power joint 1 device collected by the rotary encoder 16, the torque sensor 15, the angle sensor 17 and the current sensor 21 is combined with a control target Tref of the torque ring controller 24 to obtain a control target Tm of the current ring controller 25, so that the actual working data approaches to data meeting requirements, the effect of closed-loop control is achieved, feedback compensation is provided, and the effects of improving the sensitivity of the whole power joint 1 device and controlling high robustness are achieved.
Further, the constant coefficient A, B satisfies the following conditions:
A＝N*Jm/Cs；
B＝1/N+N*Jm/Jl；
wherein Jl is the moment of inertia of the power joint 1 and the load, and Cs is an adjustable coefficient.
Further, referring to fig. 3, the input data of the torque loop controller 24 includes data corresponding to the acquisition of the rotary encoder 16, the torque sensor 15, the angle sensor 17 and the current sensor 21 and a control target Tref of the torque loop controller 24, an output of the torque loop controller 24 is connected to the current loop controller 25, an output of the current loop controller 25 is connected to the motor 13, specifically, a processing module 243, a differential filter 241 and an expansion observer 242 are disposed in the torque loop controller 24, an error derivative de is obtained by observation of the differential filter 241 or the expansion observer 242, system interference data Z _ hat is obtained by the expansion observer 242, and the data acquired by the differential filter 241 and the expansion observer 242 are input to the processing module 243 to process the control target Tm of the output current loop controller 25.
Furthermore, the above-mentioned extended observer 242 includes a first extended observer 2421 and at least one second extended observer 2422, the first extended observer 2421 observes the rotation angle θ m of the motor 13 or a filter thereof and outputs a frictional disturbance value z13 of the motor 13, and the second extended observer 2422 outputs a load and a frictional disturbance value z23 of the upper arm 11 and the lower arm 12.
Wherein the first extended observer 2421 satisfies the following condition:
eq11：e1＝z11-θm_hat；
eq12：dz11＝z12-β10*e1；
eq13：dz12＝z13-β11*fal(e1,α11,11)+b11*Ifbk_hat+b12*Ts_hat；
eq14：dz13＝-β12*fal(e1,α12,12)；
wherein dz11, dz12 and dz13 are derivatives of z11, z12 and z13 respectively, β 10, β 11, β 12, α 11, α 12, 11 and 12 are adjustable parameters, b11 and b12 are constants set according to measured system parameters, one of the constants is b11 ═ 1/Jm, b12 ═ 1/(Jm N), and fal is a monotonic function.
The input variables of the first extended observer 2421 are θ m _ hat, Ifbk _ hat and Ts _ hat, where θ m _ hat is the rotation angle θ m of the motor 13 collected by the rotary encoder 16 or a filtered value thereof, Ifbk _ hat is the current Ifbk or a filtered value thereof collected by the current sensor 21 when the motor 13 is in operation, Ts _ hat is the relative torque Ts between the upper arm 11 and the lower arm 12 collected by the torque sensor 15 or a filtered value thereof, and the output variables are z11, z12 and z13, where z11 is close to the rotation angle θ m of the motor 13, z12 is close to a derivative value of the rotation angle θ m of the motor 13, and z13 is close to a current friction interference value of the motor 13.
The second extended observer 242 satisfies the following condition:
eq21：e2＝z21-θl_hat；
eq22：dz21＝z22-β20*e2；
eq23：dz22＝z23-β21*fal(e2,α21,21)+b22*Ts_hat；
eq24：dz23＝-β22*fal(e2,α22,22)；
the dz21, dz22 and dz23 are respectively corresponding to derivatives of z21, z22 and z23, β 20, β 21, β 22, α 21, α 22, 21 and 22 are adjustable parameters, b22 is a constant set according to a measured system parameter, one of the constants is b 22-1/Jl, Jl is the rotational inertia of the power joint 1 and a load, and fal is a monotonic function.
The input variables of the second extended observer 2422 are θ l _ hat and Ts _ hat, θ l _ hat is the relative angle θ l between the upper arm 11 and the lower arm 12 collected by the angle sensor 17 or the filtered value thereof, Ts _ hat is the relative torque Ts between the upper arm 11 and the lower arm 12 collected by the torque sensor 15 or the filtered value thereof, and the output variables are z21, z22, and z23, where z21 is close to the relative angle θ l between the upper arm 11 and the lower arm 12, z22 is close to the derivative value of the relative angle θ l between the upper arm 11 and the lower arm 12, and z23 is close to the load and frictional disturbance values of the upper arm 11 and the lower arm 12.
Furthermore, the error derivative de satisfies de dTref-dTs, wherein dTs is z32 and dTref is dz 41.
In addition, the above-described extended observer 242 includes a third extended observer 2423, wherein the third extended observer 2423 outputs z32, and the third extended observer 2423 satisfies the following condition:
eq31：e3＝z31-Ts_hat；
eq32：dz31＝z32-β30*e3；
eq33：dz32＝z33-β31*fal(e3,α31,31)+b31*Ifbk_hat-b32*Ts_hat；
eq34：dz33＝-β32*fal(e3,α32,32)；
the parameters dz31, dz32 and dz33 are derivatives of z31, z32 and z33 respectively, β 30, β 31, β 32, α 31, α 32, 31 and 32 are adjustable parameters, b31 and b32 are constants set according to measured system parameters, one of the constants is b31 ═ K/(N Jm), b32 ═ K/Jl + K/(N ═ N Jm), K is an equivalent spring stiffness coefficient from an output end of the motor 13 to the joint 1 load, Jl is the dynamic joint 1 and the load rotational inertia, fal is a monotonic function, b33 and b34 are constants set according to the measured system parameters, and the other one of the constants is b33 ═ K/N, and b 34-K.
The third extended observer 2423 has as input variables Ifbk _ hat, Ts _ hat, Ifbk _ hat being the current Ifbk or a filtered value thereof when the motor 13 is operating, Ts _ hat being the relative torque Ts between the upper arm 11 and the lower arm 12 or a filtered value thereof, and as output variables z31, z32, z33, where z31 approaches Ts and z32 approaches the derivative value of Ts.
In another embodiment, the third extended observer 2423 satisfies the following condition:
eq31：e3＝z31-Ts_hat；
eq32：dz31＝z32-β30*e3；
eq33b：dz32＝z33-β31*fal(e3,α31,31)+b31*Ifbk-b32*Ts+b33*z13+b34*z23；
eq34：dz33＝-β32*fal(e3,α32,32)；
α 31, α 32, 31 and 32 are adjustable parameters, b33 and b34 are constants set according to measured system parameters, one of the constants is b33 ═ K/N, b34 ═ K, K is the equivalent spring stiffness coefficient from the output end of the motor 13 to the load of the joint 1, Jl is the dynamic joint 1 and the load moment of inertia, and fal is a monotonic function.
The third extended observer 2423 has as input variables Ifbk _ hat, Ts _ hat, Ifbk _ hat being the current Ifbk or a filtered value thereof when the motor 13 is operating, Ts _ hat being the relative torque Ts between the upper arm 11 and the lower arm 12 or a filtered value thereof, and as output variables z31, z32, z33, where z31 approaches Ts and z32 approaches the derivative value of Ts.
Further, the differentiating filter 241 includes a first differentiating filter 2411, wherein the first differentiating filter 2411 outputs z 41; and the first differentiating filter 2411 satisfies the following condition:
eq41：e4＝z41-Tref；
eq42：dz41＝-β40*fal(e4,α42,42)；
where dz41 is the derivative of z41, β 40, α 42, 42 are adjustable parameters, fal is a monotonic function, the input variable of the first derivative filter 2411 is the control target Tref of the torque loop controller 24 or its filtered value, and the output variable is z41, dz41, where z41 approaches the control target Tref value of the torque loop controller 24 and dz41 approaches the derivative value dTref of the control target Tref of the torque loop controller 24.
Furthermore, the differential filter 241 further includes a second differential filter 2412, a third differential filter 2413, a fourth differential filter 2414, and one or more fifth differential filters 2415; the second, third, fourth and fifth differential filters 2412, 2413, 2414 and 2415 perform filtering and differential processing on at least one of the relative torque Ts between the upper arm 11 and the lower arm 12, the rotation angle θ m of the motor 13, the relative angle θ l of rotation between the upper arm 11 and the lower arm 12, and the current Ifbk when the motor 13 is operated; the second differentiating filter 2412 satisfies the following conditions:
eq51：e5＝z51-Ts；
eq52：dz51＝-β50*fal(e5,α52,52)；
where dz51 is the derivative of z51, β 50, α 52, 52 are adjustable parameters, fal is a monotonic function, the input variable is the relative torque Ts between the upper arm 11 and the lower arm 12 or a filtered value thereof, the output variables are z51, dz51, where z51 approaches the value of the relative torque Ts between the upper arm 11 and the lower arm 12, and dz51 approaches the derivative value dTs of the relative torque Ts between the upper arm 11 and the lower arm 12.
The third differential filter 2413 described above satisfies the following conditions:
eq61：e6＝z61-θm；
eq62：dz61＝-β60*fal(e6,α62,62)；
where dz61 is the derivative of z61, β 60, α 62, 62 are adjustable parameters, fal is a monotonic function, the input variable is the rotation angle θ m of the motor 13 or its filter, the output variables are z61 and dz61, z61 approaches the value of the rotation angle θ m of the motor 13, and dz61 approaches the derivative value d θ m of the rotation angle θ m of the motor 13.
The fourth differential filter 2414 described above satisfies the following conditions:
eq71：e7＝z71-θl；
eq72：dz71＝-β70*fal(e7,α72,72)；
where dz71 is the derivative of z71, β 70, α 72, 72 are adjustable parameters, fal is a monotonic function, the input variable is the relative angle θ l of rotation between the upper arm 11 and the lower arm 12 or a filtered value thereof, the output variable is z71, dz71, where z71 approaches the value of the relative angle θ l of rotation between the upper arm 11 and the lower arm 12, and dz71 approaches the value d θ l of the derivative of the relative angle θ l of rotation between the upper arm 11 and the lower arm 12.
The fifth differential filter 2415 described above satisfies the following conditions:
eq81：e8＝z81-Ifbk；
eq82：dz81＝-β80*fal(e8,α82,82)；
where dz81 is the derivative of z81, β 80, α 82, 82 are adjustable parameters, fal is a monotonic function, the input variable is the current Ifbk with motor 13 operating or a filtered value thereof, and the output variables are z81, dz81, where z81 approaches the value of current Ifbk with motor 13 operating and dz81 approaches the derivative value dlfbk of current Ifbk with motor 13 operating.
Furthermore, the above-mentioned synovial approximation law function F (S) is an equal-speed approximation law function F ═ sign (S), or an exponential approximation law function F ═ ks ═ S-sign (S), or a power approximation law function F ═ ks abs (S) ^ alpha sign (S), where ks and alpha are adjustable parameters, sign is a sign function, and abs is an absolute value function.
In this embodiment, the values of the equivalent spring stiffness coefficient K from the output end of the motor 13 to the load of the joint 1, the moment of inertia Jm of the motor 13, the reduction ratio N of the reduction gear 14 of the power joint 1, and the power joint 1 and the load moment of inertia J1 can be measured through experiments, and specifically, the values are obtained by measuring for many times to obtain an average value.
The monotonic function fal is a power function f1 or an arc tangent nonlinear function f 2;
wherein the power function f1 is expressed as:
the arctangent nonlinear function f2 is expressed as f2(e, α) β atan (2 α e/pi).
Further, the input variables of the processing module 243 are z41, z51, dz41, z32, z13 and z23, the output variable of the processing module 243 is Tm, and the processing module 243 implements the following four calculations:
e＝z41-z51；
de＝dz41-z32；
S＝Cs*de+e；
Tm＝A*(de-F(S))+B*z32+C*z13+D*z23；
wherein: f (S) ═ ks S-sign (S); ks is an adjustable parameter, a constant coefficient a is N × Jm/Cs, B is 1/N + N × Jm/Jl, C is-Jm, D is N × Jm, Jm is a rotational inertia of the motor 13, Jl is a relative rotational inertia of the upper arm 11 and the lower arm 12, N is a reduction ratio of the reducer 14, and Cs is an adjustable parameter.
The current loop controller 25 adopts a traditional PI loop to realize control, and the output torque of the driving motor 13 approaches the control target Tm output by the torque loop controller 24. The method of controlling the current loop controller 25 is mainly a PID control method or a slip film control method or an optimum control method.
The torque control system of the power joint device adopts the synovial membrane control in the signal processing unit to realize quick response, adopts the extended observer 242 to observe the system interference and obtain the high-quality derivative of the related observed quantity, adopts the differential filter 241 to filter the input data and obtain the derivative, utilizes the processing module 243 to process the control target of the output current loop controller 25 to control the output torque of the joint 1, by performing filtering, differentiation, observation and control calculation on the input data and the control target data, so as to achieve the torque control with quick response and strong robustness under the condition that the robot joint 1 has flexibility and the friction force is changed quickly, the related system of the robot or the exoskeleton with the power joint 1 device has high sensitivity and stability.
As shown in fig. 4, the present embodiment also provides a torque control method of a torque control system of a power joint device, the method including:
s1, acquiring a rotation angle θ m of the motor 13, a relative torque Ts between the upper arm 11 and the lower arm 12, a relative angle θ l of rotation between the upper arm 11 and the lower arm 12, a current Ifbk when the motor 13 is operated, and a control target Tref of the torque loop controller 24;
s2, observing or filtering θ m, θ l, Ts, obtaining an output z13 of the first extended observer 2421, an output z23 of the second extended observer 2422, an output z32 of the third extended observer 2423, and an output dz41 of the first differential filter 2411;
s3, according to e-Tref-Ts, where e is a deviation or an approximate value between the actual measurement value Ts and the control target Tref; de-dz 41-z32 or de-z 32, where de is an approximation of the derivative of the deviation e; s ═ Cs × de + e, where S is a synovial surface parameter and Cs is an adjustable coefficient; tm ═ a (de-f (s)) + B × + Ts + C × z13+ D × z23, where Tm is the torque loop controller 24 output; obtaining an output Tm of the torque loop controller 24;
s4, the current loop controller 25 controls the motor 13 to output a control torque close to Tm with Tm as a control target, and returns to the step S1.
In the step S1, the rotation angle θ m of the motor 13 is mainly acquired by the rotary encoder 16, the relative torque Ts between the upper arm 11 and the lower arm 12 is acquired by the torque sensor 15, the relative angle θ l of rotation between the upper arm 11 and the lower arm 12 is acquired by the angle sensor 17, the current Ifbk of the motor 13 during operation is acquired by the current sensor 21, and the control target Tref of the torque loop controller 24 is acquired from the control command.
For the above step S2, the first extended observer 2421, the second extended observer 2422, the third extended observer 2423 and the first differential filter 2411 are respectively used to observe or filter the input variables θ m, θ l, Ts and Tref, where θ m _ hat is equal to θ m of the first extended observer 2421, θ l _ hat is equal to θ l of the second extended observer 2422, Ts _ hat is equal to Ts of the third extended observer 2423, and Tref is input to the first differential filter 2411, so as to obtain z13 of the first extended observer 2421, z23 of the second extended observer 2422, z32 of the third extended observer 2423 and dz41 of the first differential filter 2411, respectively, where z2 is close to the derivative value of 387, z13 is close to the frictional interference value of the motor 13, and z23 is close to the load and frictional interference value of the upper and lower arms 12.
Further, after the step of S2, the method further includes:
and S21, observing the input Tref, Ts, theta m, theta l and Ifbk respectively to obtain output data z41, dz41, z51, dz51, z61, z71 and z 81.
The step S21 is to observe the input variables Tref, Ts, θ m, θ l, Ifbk by using the differential filter 241, the second differential filter 24122, the third differential filter 2413, the fourth differential filter 2414, and the fifth differential filter 2415, respectively, and obtain the output data z41, dz41, z51, dz51, z61, z71, and z81, respectively. Specifically, the tracking signal of Tref is z41, the differential signal of Tref is dz41, the tracking signal of Ts is z51, the differential signal of Ts is dz51, the tracking signal of θ m is z61, the tracking signal of θ l is z71, and the tracking signal of Ifbk is z 81; in other words, z41 approaches Tref, dz41 approaches the Tref derivative value, z51 approaches Ts, z61 approaches θ m, z71 approaches θ l, and z81 approaches Ifbk.
For the step S3, specifically, the above-mentioned e ═ Tref (or z41) -Ts (or z31 or z51), where e is the deviation of the actual measurement value Ts from the control target Tref;
de-dz 41-z32 (or dz51) or de-z 32, where de is an approximation of the derivative of the deviation e;
s ═ Cs × de + e, where S is a synovial surface parameter and Cs is an adjustable coefficient;
tm ═ a (de-f (s)) + B × + Ts + C × z13+ D × z23, where Tm is the torque loop controller 24 output value.
Most preferably, e-z 41-z 51;
de＝dz41-z32；
S＝Cs*de+e；
Tm＝A*(de-F(S))+B*z32+C*z13+D*z23；
wherein: f (S) ═ ks S-sign (S); a, B, C and D are constant coefficients set according to measured system parameters, A is N Jm/Cs, B is 1/N + N Jm/Jl, C is-Jm, D is N Jm, wherein Jm is the rotational inertia of the motor 13 of the power joint 1, N is the reduction ratio of the speed reducer 14 of the power joint 1, Jl is the rotational inertia of the power joint 1 and a load, the values of Jm, N and Jl can be measured through experiments, ks and Cs are adjustable parameters;
in the above step S4, the motor 13 is mainly controlled to drive with Tm as a target:
and calculating a vector control input voltage value according to PI control:
Ie＝Ie+Tm*Km-Ifbk；
Pout＝Kp*(Tm*Km-Ifbk)+Ki*Ie；
and controlling PWM output by taking Pout as a driving input value of the motor 13, wherein Km is a constant coefficient and can be measured by experiments.
In addition, for the above step S4, in the actual control process, a PID control method, a slip film control method, or an optimal control method is used.
In other embodiments, the steps of S21 and S2 may be performed in opposite order.
The technical contents of the present invention are further illustrated by the examples only for the convenience of the reader, but the embodiments of the present invention are not limited thereto, and any technical extension or re-creation based on the present invention is protected by the present invention. The protection scope of the invention is subject to the claims.
Claims (11)
1. The moment control system of the power joint device is characterized by comprising a signal acquisition unit and a signal processing unit, wherein the signal acquisition unit comprises a rotary encoder connected with a motor for driving the joint to move, a torque sensor for measuring the relative torque of a lower arm of an upper arm, an angle sensor positioned between the upper arm and the lower arm of the joint and a current sensor connected with the motor for driving the joint to move; the motor control system comprises a rotary encoder, a torque sensor, an angle sensor and a current sensor, wherein the rotary encoder acquires a rotation angle theta m of the motor, the torque sensor acquires a relative torque Ts between an upper arm and a lower arm, the angle sensor acquires a relative rotation angle theta l between the upper arm and the lower arm, and the current sensor acquires a current Ifbk when the motor works; the signal processing unit comprises a memory and a processor, a torque loop controller and a current loop controller are arranged in the processor, and the output of the torque loop controller is a control target Tm of the current loop controller;
and Tm ═ a (de-f (s)) + B ═ T _ hat + Z _ hat;
a, B is a constant coefficient determined according to the system model; de is an error derivative value, the error e is equal to Tref-Ts or filtered data thereof, and Tref is a control target of the moment ring controller; f (S) is a synovial membrane approximation law function, wherein S ═ Cs × de + e, and Cs is an adjustable coefficient; t _ hat is Ts or data after filtering processing; z _ hat is system disturbance data, the system disturbance data Z _ hat includes a motor frictional disturbance value Z13 and/or a load and frictional disturbance value Z23 of the upper arm and the lower arm, the system disturbance data Z _ hat is an optional item, and the system disturbance data Z _ hat satisfies C × Z13 or D × Z23 or the sum of C × Z1 and D × Z23, C ═ Jm, D ═ N ═ Jm, Jm is the rotational inertia of the motor, and N is the reduction gear ratio of the power joint.
2. The torque control system of a powered joint arrangement of claim 1, wherein said constant coefficient A, B satisfies:
A＝N*Jm/Cs；
B＝1/N+N*Jm/Jl；
wherein Jl is the moment of inertia of the power joint and the load, and Cs is an adjustable coefficient.
3. The torque control system according to claim 1, wherein a processing module, a differential filter and an extended observer are provided in the torque loop controller, the derivative error value de is obtained by observation of the differential filter or the extended observer, the system disturbance data Z _ hat is obtained by the extended observer, and data obtained by the differential filter and the extended observer are input to the processing module to process the control target Tm of the output current loop controller.
4. The torque control system according to claim 3, wherein the extended observer includes a first extended observer that observes a rotation angle θ m of the motor or a filtered value thereof and outputs a motor frictional disturbance value z13, and at least one second extended observer that outputs a load of the upper and lower arms and a frictional disturbance value z 23; wherein the first extended observer satisfies the following condition:
eq11：e1＝z11-θm_hat；
eq12：dz11＝z12-β10*e1；
eq13：dz12＝z13-β11*fal(e1,α11,11)+b11*Ifbk_hat+b12*Ts_hat；
eq14：dz13＝-β12*fal(e1,α12,12)；
wherein dz11, dz12 and dz13 are respectively derivatives of z11, z12 and z13, β 10, β 11, β 12, α 11, α 12, 11 and 12 are adjustable parameters, b11 and b12 are constants set according to the measured system parameters, one of the constants is b11 ═ 1/Jm, b12 ═ 1/(Jm N), and fal is a monotonic function;
the input variables of the first extended observer are thetam _ hat, Ifbk _ hat and Ts _ hat, wherein thetam _ hat is a rotating angle thetam of the motor collected by a rotary encoder or a filter value thereof, Ifbk _ hat is a current Ifbk of the motor collected by a current sensor when the motor works or a filter value thereof, Ts _ hat is a relative torque Ts between an upper arm and a lower arm collected by a torque sensor or a filter value thereof, and the output variables are z11, z12 and z13, wherein z11 approaches the rotating angle thetam of the motor, z12 approaches a derivative value of the rotating angle thetam of the motor, and z13 approaches a current motor friction interference value;
the second extended observer satisfies the following condition:
eq21：e2＝z21-θl_hat；
eq22：dz21＝z22-β20*e2；
eq23：dz22＝z23-β21*fal(e2,α21,21)+b22*Ts_hat；
eq24：dz23＝-β22*fal(e2,α22,22)；
the dz21, dz22 and dz23 are respectively corresponding to derivatives of z21, z22 and z23, β 20, β 21, β 22, α 21, α 22, 21 and 22 are adjustable parameters, b22 is a constant set according to a measured system parameter, wherein one of the constants is b22 ═ 1/Jl, Jl is the rotational inertia of the power joint and the load, and fal is a monotonic function;
the input variables of the second extended observer are theta l _ hat and Ts _ hat, theta l _ hat is a relative angle theta l between an upper arm and a lower arm collected by an angle sensor or a filter value of the relative angle theta l, Ts _ hat is a relative torque Ts between an upper arm and a lower arm collected by a torque sensor or a filter value of the relative torque Ts, and the output variables are z21, z22 and z23, wherein z21 approaches the relative angle theta l between the upper arm and the lower arm, z22 approaches a derivative value of the relative angle theta l between the upper arm and the lower arm, and z23 approaches a load and friction interference value of the upper arm and the lower arm.
5. The torque control system of a power joint device according to claim 3, wherein the error derivative value de satisfies de ═ dTRef-dTs, wherein dTs ═ z32, dTRef ═ dz 41;
the extended observer includes a third extended observer, wherein the third extended observer outputs z32, the third extended observer satisfying the following condition:
eq31：e3＝z31-Ts_hat；
eq32：dz31＝z32-β30*e3；
eq33：dz32＝z33-β31*fal(e3,α31,31)+b31*Ifbk_hat-b32*Ts_hat；
or eq33b dz32 ═ z33- β 31 ═ fal (e3, α 31,31) + b31 × Ifbk-b32 × + b33 × z13+ b34 × z 23;
eq34：dz33＝-β32*fal(e3,α32,32)；
wherein dz31, dz32 and dz33 are respectively corresponding to derivatives of z31, z32 and z33, β 30, β 31, β 32, α 31, α 32, 31 and 32 are adjustable parameters, b31 and b32 are constants set according to measured system parameters, one of the constants is b31 ═ K/(N Jm), b32 ═ K/Jl + K/(N ═ N Jm), K is an equivalent spring stiffness coefficient from an output end of the motor to a joint load, Jl is a dynamic joint and load rotational inertia, fal is a monotonic function, b33 and b34 are constants set according to the measured system parameters, one of the constants is b33 ═ K/N, and b34 ═ K;
the third extended observer input variables are Ifbk _ hat, Ts _ hat, Ifbk _ hat, which is the current Ifbk or a filtered value thereof during operation of the motor, Ts _ hat is the relative torque Ts between the upper and lower arms or a filtered value thereof, and the output variables are z31, z32, z33, where z31 approaches Ts and z32 approaches the derivative value of Ts.
6. The torque control system of a powered joint arrangement of claim 3, said derivative filter comprising a first derivative filter, wherein said first derivative filter outputs z 41; and the first differential filter satisfies the following condition:
eq41：e4＝z41-Tref；
eq42：dz41＝-β40*fal(e4,α42,42)；
wherein dz41 is a derivative of z41, β 40, α 42, 42 are adjustable parameters, fal is a monotonic function, the input variable of the first differential filter is a control target Tref of the torque loop controller or a filter value thereof, the output variable is z41 and dz41, wherein z41 approaches a control target Tref value of the torque loop controller, and dz41 approaches a derivative value dTref of the control target Tref of the torque loop controller.
7. The torque control system of a powered joint arrangement of claim 3, wherein said derivative filter further comprises a second derivative filter, a third derivative filter, a fourth derivative filter, one or more fifth derivative filters; the second differential filter, the third differential filter, the fourth differential filter and the fifth differential filter respectively perform filtering and differential processing on at least one of a relative torque Ts between the upper arm and the lower arm, a rotation angle thetam of the motor, a relative angle thetal between the upper arm and the lower arm, and a current Ifbk when the motor works; wherein the second derivative filter satisfies the following condition:
eq51：e5＝z51-Ts；
eq52：dz51＝-β50*fal(e5,α52,52)；
wherein dz51 is a derivative of z51, β 50, α 52, 52 are adjustable parameters, fal is a monotonic function, the input variable is the relative torque Ts between the upper and lower arms or a filtered value thereof, the output variables are z51, dz51, wherein z51 approaches the value of the relative torque Ts between the upper and lower arms, and dz51 approaches the derivative value dTs of the relative torque Ts between the upper and lower arms;
the third derivative filter satisfies the following condition:
eq61：e6＝z61-θm；
eq62：dz61＝-β60*fal(e6,α62,62)；
wherein dz61 is a derivative of z61, β 60, α 62, 62 are adjustable parameters, fal is a monotonic function, an input variable is a rotation angle theta m of the motor or a filter value thereof, output variables are z61 and dz61, z61 approaches a value of the rotation angle theta m of the motor, and dz61 approaches a derivative value d theta m of the rotation angle theta m of the motor;
the fourth differential filter satisfies the following condition:
eq71：e7＝z71-θl；
eq72：dz71＝-β70*fal(e7,α72,72)；
wherein dz71 is a derivative of z71, β 70, α 72, 72 are adjustable parameters, fal is a monotonic function, an input variable is a relative angle theta l of rotation between the upper arm and the lower arm or a filtered value thereof, an output variable is z71, dz71, wherein z71 approaches a numerical value of the relative angle theta l of rotation between the upper arm and the lower arm, and dz71 approaches a derivative value d theta l of the relative angle theta l of rotation between the upper arm and the lower arm;
the fifth differential filter satisfies the following condition:
eq81：e8＝z81-Ifbk；
eq82：dz81＝-β80*fal(e8,α82,82)；
where dz81 is the derivative of z81, β 80, α 82, 82 are adjustable parameters, fal is a monotonic function, the input variable is the current Ifbk during motor operation or a filtered value thereof, and the output variables are z81, dz81, where z81 approximates the value of the current Ifbk during motor operation and dz81 approximates the derivative value dlfbk of the current Ifbk during motor operation.
8. The torque control system according to any one of claims 1 to 7, wherein the synovial approach law function F (S) is an equal-velocity approach law function F ═ -sign (S), or an exponential approach law function F ═ -ks S-sign (S), or a power-order approach law function F ═ -ks abs (S) or S ^ alpha sign (S), wherein ks and alpha are adjustable parameters, sign is a sign function, and abs is an absolute value function.
9. The torque control system according to any one of claims 4 to 7, wherein the monotonic function fal function is a power function f1 or an arctangent nonlinear function f 2;
wherein the power function f1 is expressed as:
the expression of the arctangent nonlinear function f2 is as follows, f2(e, α) is β × atan (2 α e/pi), wherein e is Tref-Ts or filtered data thereof, Tref is a control target of a torque loop controller, Ts is relative torque between an upper arm and a lower arm acquired by a torque sensor, and β and α are adjustable parameters.
10. A torque control method of a torque control system of a power joint device, characterized by comprising:
acquiring a rotation angle theta m of the motor, a relative torque Ts between an upper arm and a lower arm, a relative rotation angle theta l between the upper arm and the lower arm, a current Ifbk when the motor works and a control target Tref of a torque loop controller;
observing or filtering the thetam, thetal and Ts to obtain an output z13 of the first extended observer, an output z23 of the second extended observer, an output z32 of the third extended observer and an output dz41 of the first differential filter; wherein z13 is a motor friction interference value; z23 is the load and frictional interference value of the upper and lower arms; z32 is the output variable of the third extended observer, z32 approaches the derivative value of the actual measured value Ts; dz41 is the output variable of the first derivative filter, dz41 approaches the derivative value dTRef of the control target Tref of the torque loop controller;
according to e-Tref-Ts, wherein e is the deviation or the approximate value of the actual measurement value Ts and the control target Tref; de-dz 41-z32 or de-z 32, where de is an approximation of the derivative of the deviation e; s ═ Cs × de + e, where S is a synovial surface parameter and Cs is an adjustable coefficient; tm ═ a (de-f (s)) + B × + Ts + C × z13+ D × z23, where Tm is the torque loop controller output value; obtaining an output Tm of a torque ring controller, wherein F (S) is a sliding mode approximation law function, F (S) is a constant velocity approximation law function F ═ -sign (S), or an exponential approximation law function F ═ -ks S-sign (S), or a power approximation law function F ═ -ks abs (S), and ks, alpha are adjustable parameters, sign is a sign function, and abs is an absolute value function; ts is the relative torque between the upper and lower arms; a is N Jm/Cs; b is 1/N + N Jm/Jl; jl is the moment of inertia of the power joint and the load, and Cs is an adjustable coefficient; c is-Jm, D is N Jm, Jm is the moment of inertia of the motor, and N is the reduction ratio of the reduction mechanism of the power joint;
and the current loop controller takes Tm as a control target, controls the output control torque of the motor to approach Tm, and returns to the step of acquiring the rotation angle theta m of the motor, the relative torque Ts between the upper arm and the lower arm, the relative rotation angle theta l between the upper arm and the lower arm, the current Ifbk when the motor works and the control target Tref of the torque loop controller.
11. The torque control method of a torque control system of a dynamic joint device according to claim 10, wherein the step of observing or filtering θ m, θ l, Ts to obtain the output z13 of the first extended observer, the output z23 of the second extended observer, the output z32 of the third extended observer, and the output dz41 of the first differential filter further comprises:
respectively observing input Tref, Ts, thetam, thetal and Ifbk to obtain output data z41, dz41, z51, dz51, z61, z71 and z 81;
z41 is the output variable of the first differential filter, z41 approaches the control target Tref value of the torque loop controller; z51 is the output variable of the second differential filter, z51 approaches the value of the relative torque Ts between the upper and lower arms; dz51 is the output variable of the second derivative filter, dz51 is the derivative of z51, dz51 approaches the derivative value dTs of the relative torque Ts between the upper and lower arms; z61 is the output variable of the third differential filter, and z61 approaches the value of the rotation angle thetam of the motor; z71 is the output variable of the fourth differential filter, z71 approaches the value of the relative angle thetal of the rotation between the upper arm and the lower arm; z81 is the output variable of the fifth differential filter, z81 approximates the value of the current Ifbk when the motor is operating.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710481780.XA CN109108954B (en) | 2017-06-22 | 2017-06-22 | Torque control system and torque control method of power joint device |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710481780.XA CN109108954B (en) | 2017-06-22 | 2017-06-22 | Torque control system and torque control method of power joint device |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109108954A CN109108954A (en) | 2019-01-01 |
| CN109108954B true CN109108954B (en) | 2020-07-21 |
Family
ID=64732831
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710481780.XA Active CN109108954B (en) | 2017-06-22 | 2017-06-22 | Torque control system and torque control method of power joint device |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109108954B (en) |
Families Citing this family (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20220226993A1 (en) * | 2019-04-17 | 2022-07-21 | Universal Robots A/S | Method of controlling a robot arm based on adaptive friction |
| CN110027002B (en) * | 2019-04-24 | 2021-09-17 | 深圳市智能机器人研究院 | Multi-motor drive based bionic joint control system and method |
| CN112809651B (en) * | 2019-11-15 | 2021-10-29 | 深圳市英汉思动力科技有限公司 | Power-assisted exoskeleton control method, power-assisted exoskeleton control system and computer equipment |
| CN111300423B (en) * | 2020-03-17 | 2021-05-18 | 北京理工大学 | Robot joint torque control system and load compensation method thereof |
| CN114533488B (en) * | 2022-02-18 | 2023-09-26 | 山东泽普医疗科技有限公司 | Multi-joint constant speed training control method and system |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102208891A (en) * | 2010-11-18 | 2011-10-05 | 东南大学 | Method for controlling PMSM (permanent magnet synchronous motor) servo system based on friction and disturbance compensation |
| CN104483899A (en) * | 2014-11-12 | 2015-04-01 | 中国科学院长春光学精密机械与物理研究所 | Arm type space astronomical telescope inertial pointing control method and control system |
| CN106452206A (en) * | 2016-06-30 | 2017-02-22 | 杭州电子科技大学 | Sliding mode adaptive controller with built-in brushless DC motor current loop control for two-wheeled self-balancing robot |
| CN106483964A (en) * | 2015-08-31 | 2017-03-08 | 中南大学 | A kind of robot Shared control method based on contact force observer |
| CN106625683A (en) * | 2017-02-27 | 2017-05-10 | 哈尔滨工业大学深圳研究生院 | Reconfigurable man-machine cooperation mechanical arm system based on WIFI |
-
2017
- 2017-06-22 CN CN201710481780.XA patent/CN109108954B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102208891A (en) * | 2010-11-18 | 2011-10-05 | 东南大学 | Method for controlling PMSM (permanent magnet synchronous motor) servo system based on friction and disturbance compensation |
| CN104483899A (en) * | 2014-11-12 | 2015-04-01 | 中国科学院长春光学精密机械与物理研究所 | Arm type space astronomical telescope inertial pointing control method and control system |
| CN106483964A (en) * | 2015-08-31 | 2017-03-08 | 中南大学 | A kind of robot Shared control method based on contact force observer |
| CN106452206A (en) * | 2016-06-30 | 2017-02-22 | 杭州电子科技大学 | Sliding mode adaptive controller with built-in brushless DC motor current loop control for two-wheeled self-balancing robot |
| CN106625683A (en) * | 2017-02-27 | 2017-05-10 | 哈尔滨工业大学深圳研究生院 | Reconfigurable man-machine cooperation mechanical arm system based on WIFI |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109108954A (en) | 2019-01-01 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109108954B (en) | Torque control system and torque control method of power joint device | |
| CN109664297B (en) | Vibration suppression method, system and device for robot and computer readable memory | |
| KR101495949B1 (en) | Reducer abnormality determination method, abnormality determination device, and robot system | |
| KR101417488B1 (en) | Method and system for extracting intended torque for wearable robot | |
| CN106842905B (en) | PID control method and device | |
| JP6305283B2 (en) | Power system control system | |
| JP6214948B2 (en) | Friction compensation device, friction compensation method, and servo control device | |
| CN113799136B (en) | Robot joint high-precision control system and method based on full-state feedback | |
| KR20160117223A (en) | Apparatus for controlling servo motor and method for detecting collision | |
| SE516720C2 (en) | Equipment for controlling an industrial robot and a procedure for programming and / or adjusting the movement of the robot | |
| CN111487871A (en) | Acceleration control method for load on power backpack based on disturbance observer | |
| Madsen et al. | Model-based on-line estimation of time-varying nonlinear joint stiffness on an e-series universal robots manipulator | |
| RU2394674C2 (en) | Self-adaptive electric drive of robot | |
| JP2016089961A (en) | Electrostatic hydraulic actuator and parameter estimation method in electrostatic hydraulic actuator | |
| CN111070203B (en) | Control system, control method, and control program | |
| US9199377B2 (en) | Method and system for extracting intended torque for wearable robot | |
| KR101053205B1 (en) | Motor control unit with backlash compensation | |
| CN113711138A (en) | Servo control device | |
| CN113143465A (en) | Mechanical arm dragging, guiding and positioning method based on joint torque | |
| CN109465827B (en) | Single-feedback single-drive rigid-flexible coupling platform control method | |
| JP2021005918A (en) | Controller for evaluating inertia and evaluation method of inertia | |
| RU2359306C2 (en) | Self-adapting electric drive of robot | |
| CN113093549B (en) | Compound control method of multi-axis numerical control equipment | |
| RU2443543C1 (en) | Manipulator electric drive | |
| Shiferaw et al. | High performance control of Stewart platform manipulator using sliding mode control with synchronization error |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |