CN109108954A - A kind of the Torque Control system and its Torque Control method of power joint arrangement - Google Patents
A kind of the Torque Control system and its Torque Control method of power joint arrangement Download PDFInfo
- Publication number
- CN109108954A CN109108954A CN201710481780.XA CN201710481780A CN109108954A CN 109108954 A CN109108954 A CN 109108954A CN 201710481780 A CN201710481780 A CN 201710481780A CN 109108954 A CN109108954 A CN 109108954A
- Authority
- CN
- China
- Prior art keywords
- torque
- hat
- motor
- value
- lower arm
- 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.)
- Granted
Links
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
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Human Computer Interaction (AREA)
- Manipulator (AREA)
Abstract
The present invention relates to a kind of Torque Control system of power joint arrangement and its Torque Control method, which includes signal acquisition unit and signal processing unit, and signal acquisition unit includes rotary encoder, torque sensor, angular transducer and current sensor；Rotary encoder acquires the rotational angle θ m of motor, and torque sensor acquires the opposing torque Ts between upper arm and lower arm, the relative angle θ l rotated between angular transducer acquisition upper arm and lower arm, and current sensor acquires electric current Ifbk when motor work；Signal processing unit includes memory and processor, and processor includes torque ring controller and current loop controller, and the output of torque ring controller is the control target Tm of current loop controller；And Tm=A* (de-F (S))+B*T_hat+Z_hat.The present invention is joint of robot band is flexible, under the quick situation of change of frictional force, reaches quick response, the Torque Control of strong robustness.
Description
Technical field
The present invention relates to power joint arrangements, more specifically refer to a kind of power joint arrangement Torque Control system and
Its Torque Control method.
Background technique
Robot is to automatically control machine (Robot) to be commonly called as, automatically control machine include all simulation human behaviors or
The machinery (such as robot dog, Doraemon etc.) of thought and simulation other biological.Current robot neck widely applies power pass
Section, power joint can be used for simulating joint extension or the bending of human body, wherein the exoskeleton robot wearable for human body
Multiple power joints are generally had, power joint has upper arm, lower arm, motor and speed reducer, and upper and lower arms are in motor and subtract
Can stretch or be bent under the driving of fast machine, typically with multiple sensors on power joint, including joint torsion sensor, on
Lower arm angle sensor, motor rotary encoder and motor current sensor, for obtaining corresponding data, to feed back to control
Device processed is a kind of complete for being responsible for carrying as the foundation data adjusted, such as disclosed in Chinese patent 201611165523.7
Body ectoskeleton assistant robot also is provided with the sensors such as arm angle sensor up and down in the robot that the patent refers to, utilizes angle
The angle value for spending sensor measurement joint, controls corresponding joint by central control unit according to the angle value measured and is transported
It is dynamic.
Since the joint that the joint of robot is imitation human body is formulated, exist certainly with the joint of human body in terms of flexibility
Difference is introduced with this to the articular system of robot soft therefore, it is necessary to be embedded in force snesor in the power joint to robot
Property, the joint transmission of robot generally use harmonic wave speed reducing machine to slow down, also further can bring flexibility to articular system, so that
Joint, which has flexible characteristic, can absorb vibration, slow down impact, but due to bringing obvious lag using harmonic reduction chance, cause
It controls difficulty to increase, using the ineffective of classical PID control, for example, Chinese patent 201410494011.X, which is provided, to be had
The flexible driving unit for robot joint control method of tension and joint position feedback, this method mainly realize joint of robot
System frequency is improved with flexible driving device tension feedback and joint full closed loop control to reduce flexible driving joint control error
It rings, feedforward control is particularly designed according to flexible deformation formula according to proposition, pass through motor angle and estimate design of feedback control
Device realizes control using the control method of variable coefficient PID.But the control method that above-mentioned patent refers to is still by traditional PI D
It improves, entire articular system control response speed is not good enough.In addition, Chinese patent 201510712869.3 provides one kind for controlling
The method for making wearable robot, this method are as follows: when the angle of joint of robot is more than the predetermined angular range R allowed,
Allow that the angle of joint of robot in angular range R back to described, is in make joint of robot when controlling joint of robot
Angle return to and allow in angular range R and when the angle of joint of robot reaches predetermined angular, in joint of robot
Generating makes the angle of joint of robot keep the torque of predetermined amount of time, that is, control joint angles one at the predetermined place of telling somebody what one's real intentions are
Determine in range, the control embodiment referred to is PID control or STATE FEEDBACK CONTROL, but does not disclose the side of control specific implementation
Formula.
In existing control technology, in addition to traditional PID control, modern control theory has variable-structure control, STATE FEEDBACK CONTROL
Deng these methods are required to obtain the quantity of state of system than traditional PI D fast response time, strong robustness, these quantity of states one
As be difficult to directly measure and obtain, need to observe or calculate estimation, this often leads to the error of control, and it is unstable to even result in control
Fixed, in addition, power joint generally has frictional force during exercise, frictional force belongs to quickly variation interference, this also gives accurate control
Difficulty is brought, there is the interference observer scheme for frictional force observation in the prior art, is shielded and is interfered using low-pass filter,
It is not strong to fast-changing interference rejection capability.
Therefore, it is necessary to design a kind of Torque Control system of power joint arrangement, had with realizing in joint of robot
Under flexible, the quick situation of change of frictional force, reach quick response, the Torque Control of strong robustness, so that filling with dynamic joint
The related system of the robot or ectoskeleton set has highly sensitive and stability.
Summary of the invention
It is an object of the invention to overcome the deficiencies of existing technologies, a kind of Torque Control system of power joint arrangement is provided
And its Torque Control method.
To achieve the above object, the invention adopts the following technical scheme: a kind of Torque Control system of power joint arrangement,
Including signal acquisition unit and signal processing unit, wherein signal acquisition unit includes connecting with the motor of driving joint motions
The rotary encoder that connects, measurement are located at the torque sensor of upper arm and lower arm opposing torque, between joint upper arm and lower arm
Angular transducer and with the current sensor that connect of motor of driving joint motions；Wherein, rotary encoder acquires motor
Rotational angle θ m, torque sensor acquires the opposing torque Ts between upper arm and lower arm, and angular transducer acquires upper arm under
The relative angle θ l rotated between arm, current sensor acquire electric current Ifbk when motor work；Signal processing unit includes depositing
Reservoir and processor, processor is interior to be equipped with torque ring controller and current loop control, and the output of torque ring controller is electric current
The control target Tm of ring controller；
And Tm=A* (de-F (S))+B*T_hat+Z_hat；
Wherein, A, B are the constant coefficient determined according to system model；De is error derivative value, and error e is equal to Tref-Ts
Or its filtered data, Tref are the control target of torque ring controller；F (S) is synovial membrane Reaching Law function, wherein S=Cs*de
+ e, Cs are adjustability coefficients；T_hat is Ts or its data after being filtered；Z_hat is system interference data, system interference number
It include load and the friction interference value z23 of motor friction interference value z13 and/or upper arm and lower arm, system interference number according to Z_hat
It is option according to Z_hat, meets C*z13 or D*z23 or the sum of C*z1 and D*z23, C=-Jm, D=N*Jm, Jm is electricity
The rotary inertia of machine, N are the deceleration mechanism reduction ratio in power joint.
Its further technical solution are as follows: described constant coefficient A, B meet:
A=N*Jm/Cs；
B=1/N+N*Jm/Jl；
Wherein, Jl is the rotary inertia of power joint and load, and Cs is adjustability coefficients.
Its further technical solution are as follows: be equipped with processing module, differential filter and expansion in the torque ring controller and see
Device is surveyed, error derivative value de is obtained by differential filter or expansion observer observation, it is dry to obtain system by expansion observer
Data Z_hat is disturbed, the data that the differential filter and expansion observer obtain are input to the processing module, are located
The control target Tm of reason output current loop controller.
Its further technical solution are as follows: the expansion observer includes the first expansion observer and at least one second expansion
Observer is opened, the first expansion observer is observed the rotational angle θ m of motor or its filter value, output motor friction
Power interference value z13, the load of the second expansion observer output upper arm and lower arm and friction interference value z23；Wherein, described
One expansion observer meets the following conditions:
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)；
The wherein derivative that dz11, dz12, dz13 respectively correspond as z11, z12, z13, β 10, β 11, β 12, α 11, α 12, δ
11, δ 12 is adjustable parameter, and b11, b12 are the constant according to system under test (SUT) parameter setting, and one of which is selected as b11=1/Jm,
B12=-1/ (Jm*N), fal are monotonic function；
The input variable of the first expansion observer is θ m_hat, Ifbk_hat and Ts_hat, wherein θ m_hat is
Rotary encoder acquires the rotational angle θ m or its filter value of motor, and Ifbk_hat is that current sensor acquires when motor works
Electric current Ifbk or its filter value, Ts_hat are the opposing torque Ts or its filtering between torque sensor acquisition upper arm and lower arm
Value, output variable z11, z12, z13, wherein rotational angle the θ m, z12 that z11 approaches motor approach the rotational angle θ m of motor
Derivative value, z13 level off to current motor friction interference value；
The second expansion observer meets the following conditions:
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 wherein derivative that dz21, dz22, dz23 respectively correspond as z21, z22, z23, β 20, β 21, β 22, α 21, α 22, δ
21, δ 22 is adjustable parameter, and b22 is the constant according to system under test (SUT) parameter setting, and one of which is selected as b22=1/Jl, and Jl is
The rotary inertia of power joint and load, fal are monotonic function；
The input variable of the second expansion observer is θ l_hat and Ts_hat, and θ l_hat is in angular transducer acquisition
Relative angle θ l or its filter value between arm and lower arm, Ts_hat are opposite between torque sensor acquisition upper arm and lower arm
Torque T s or its filter value, output variable z21, z22, z23, wherein z21 approaches the relative angle θ l between upper arm and lower arm,
Z22 approaches the derivative value of the relative angle θ l between upper arm and lower arm, and z23 levels off to the load and friction of upper arm and lower arm and interferes
Value.
Its further technical solution are as follows: the error derivative value de meets de=dTref-dTs, wherein dTs=z32,
DTref=dz41；
The expansion observer includes third expansion observer, wherein the third expansion observer exports z32, described
Third expansion observer meets the following conditions:
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*Ts+b33*z13+b34*
z23；
Eq34:dz33=- β 32*fal (e3, α 32, δ 32)；
The wherein derivative that dz31, dz32, dz33 respectively correspond as z31, z32, z33, β 30, β 31, β 32, α 31, α 32, δ
31, δ 32 is adjustable parameter, and b31, b32 are the constant according to system under test (SUT) parameter setting, and one of which is selected as b31=K/ (N*
Jm), b32=K/Jl+K/ (N*N*Jm), K are the output end of motor to the joint loads equivalent spring coefficient of stiffiness, and Jl is power pass
Section and load rotating inertia, fal are monotonic function；B33, b34 are the constant according to system under test (SUT) parameter setting, one of which choosing
It is selected as b33=K/N, b34=-K；
It is Ifbk_hat, Ts_hat that third, which expands observer input variable, and Ifbk_hat is electric current when motor works
Ifbk or its filter value, Ts_hat the opposing torque Ts or its filter value between upper arm and lower arm, output variable z31,
Z32, z33, wherein z31 approaches Ts, and z32 approaches the derivative value of Ts.
Its further technical solution are as follows: the differential filter includes the first differential filter, wherein first differential
Filter exports z41；And first differential filter meets the following conditions:
Eq41:e4=z41-Tref；
Eq42:dz41=- β 40*fal (e4, α 42, δ 42)；
Wherein, dz41 is the derivative of z41, and β 40, α 42, δ 42 is adjustable parameter, and fal is monotonic function, the first differential filter
Device input variable is the control target Tref or its filter value of torque ring controller, and output variable z41, dz41, wherein z41 becomes
The control target Tref numerical value of nearly torque ring controller, dz41 approach the derivative value of the control target Tref of torque ring controller
dTref。
Its further technical solution are as follows: the differential filter further include the second differential filter, third differential filter,
4th differential filter, one or more 5th differential filters；Second differential filter, third differential filter,
Four differential filters and the 5th differential filter respectively correspond the rotation to opposing torque Ts, motor between upper arm and lower arm
At least one in electric current Ifbk when relative angle θ l, the motor rotated between angle, θ m, upper arm and lower arm works is filtered
And differential process；Wherein, second differential filter meets the following conditions:
Eq51:e5=z51-Ts；
Eq52:dz51=- β 50*fal (e5, α 52, δ 52)；
Wherein, dz51 is the derivative of z51, and β 50, α 52, δ 52 is adjustable parameter, and fal is monotonic function, and input variable is upper
Opposing torque Ts or its filter value between arm and lower arm, output variable z51, dz51, wherein z51 approach upper arm and lower arm it
Between opposing torque Ts numerical value, dz51 approaches the derivative value dTs of the opposing torque Ts between upper arm and lower arm；
Third differential filter meets the following conditions:
Eq61:e6=z61- θ m；
Eq62:dz61=- β 60*fal (e6, α 62, δ 62)；
Wherein, dz61 is the derivative of z61, and β 60, α 62, δ 62 is adjustable parameter, and fal is monotonic function, and input variable is electricity
The rotational angle θ m or its filter value of machine, output variable are z61 and dz61, and z61 approaches the rotational angle θ m numerical value of motor,
Dz61 approaches the derivative value d θ m of the rotational angle θ m of motor；
4th differential filter meets the following conditions:
Eq71:e7=z71- θ l；
Eq72:dz71=- β 70*fal (e7, α 72, δ 72)；
Wherein, dz71 is the derivative of z71, and β 70, α 72, δ 72 is adjustable parameter, and fal is monotonic function, and input variable is upper
The relative angle θ l or its filter value rotated between arm and lower arm, output variable z71, dz71, wherein z71 approaches upper arm under
The relative angle θ l numerical value rotated between arm, the derivative value d θ l of the relative angle θ l rotated between dz71 approach upper arm and lower arm；
5th differential filter meets the following conditions:
Eq81:e8=z81-Ifbk；
Eq82:dz81=- β 80*fal (e8, α 82, δ 82)；
Wherein dz81 is the derivative of z81, and β 80, α 82, δ 82 is adjustable parameter, and fal is monotonic function, and input variable is electricity
Electric current Ifbk or its filter value when machine works, output variable z81, dz81, wherein z81 approaches electric current when motor works
Ifbk numerical value, dz81 approach the derivative value dIfbk of electric current Ifbk when motor works.
Its further technical solution are as follows: the synovial membrane Reaching Law function F (S) is constant speed Reaching Law function F=-sign (S) *
ε perhaps exponentially approaching rule function F=-ks*S-sign (S) * ε or power Reaching Law function F=-ks*abs (S) ^alpha*
Sign (S), wherein ε, ks, alpha are adjustable parameter, and sign is sign function, and abs is ABS function.
Its further technical solution are as follows: the monotonic function fal function is power function f1, or non-linear for arc tangent
Function f2；
Wherein, the power function f1 expression formula are as follows:
The arc tangent nonlinear function f2 expression formula is as follows: f2 (e, α, β)=β * atan (2 α e/ π).
The present invention also provides the Torque Control method of the Torque Control system of kind of power joint arrangement, the method packets
It includes:
It obtains and rotates between the opposing torque Ts between rotational angle θ m of motor, upper arm and lower arm, upper arm and lower arm
The control target Tref of electric current Ifbk and torque ring controller when relative angle θ l, motor work；
The θ m, θ l, Ts are observed or filter, the output z13 of acquisition the first expansion observer, second expand the defeated of observer
The output dz41 of z23, the output z32 of third expansion observer and the first differential filter out；
According to e=Tref-Ts, wherein e is actual measured value Ts and controls the deviation or approximation of target Tref；De=
Dz41-z32 or de=-z32, wherein de is the derivative approximation of deviation e；S=Cs*de+e, wherein S is synovial membrane face parameter,
Cs is adjustability coefficients；Tm=A* (de-F (S))+B*Ts+C*z13+D*z23, wherein Tm is torque ring controller output valve；It obtains
The output Tm of torque ring controller；
Current loop controller is control target with Tm, and control motor output control moment approaches Tm, and returns to the acquisition
Opposing torque Ts, upper arm between rotational angle θ m of motor, upper arm and lower arm and relative angle θ l, the electricity rotated between lower arm
The step of control target Tref of electric current Ifbk and torque ring controller when machine works.
Its further technical solution are as follows: observe or filter the θ m, θ l, Ts, obtain the output of the first expansion observer
The output for exporting z32 and the first differential filter of z13, the output z23 of the second expansion observer, third expansion observer
After the step of dz41 further include:
Tref, Ts, θ m, θ l, the Ifbk of input are observed respectively, obtain output data z41, dz41, z51, dz51,
z61、z71、z81。
Compared with the prior art, the invention has the advantages that: a kind of Torque Control system of power joint arrangement of the invention
System, by the way that rotary encoder, angular transducer, torque sensor and current sensor, letter are arranged on power joint arrangement
Use synovial membrane control to realize quick response in number processing unit, interferes using observer observation system is expanded and obtain related sight
The high quality derivative of measurement, is filtered using differential filter and is obtained to input data derivative, handled using processing module
The control target of current loop controller is exported, to control the output torque in joint, by input data and control target data
Be filtered, differential, observation and control calculate, to realize joint of robot band is flexible, the quick situation of change of frictional force
Under, reach quick response, the Torque Control of strong robustness, so that the correlation of robot or ectoskeleton with dynamic joint arrangement
System has highly sensitive and stability.
The invention will be further described in the following with reference to the drawings and specific embodiments.
Detailed description of the invention
Fig. 1 is the cutting structural schematic diagram for the power joint arrangement that the specific embodiment of the invention refers to；
Fig. 2 is that a kind of workflow of the Torque Control system for power joint arrangement that the specific embodiment of the invention refers to is shown
It is intended to；
Fig. 3 is a kind of structural block diagram of the Torque Control system for power joint arrangement that the specific embodiment of the invention provides；
Fig. 4 is a kind of Torque Control side of the Torque Control system for power joint arrangement that the specific embodiment of the invention provides
The flow chart of method.
Specific embodiment
In order to more fully understand technology contents of the invention, combined with specific embodiments below to technical solution of the present invention into
One step introduction and explanation, but not limited to this.
Specific embodiment as shown in figures 1-4, a kind of Torque Control system of power joint arrangement provided in this embodiment
System, can be used in robot field, especially suitable for exoskeleton robot, realize in joint of robot 1 with flexible, friction
Under the quick situation of change of power, reach quick response, the Torque Control of strong robustness, so that the machine with dynamic 1 device of joint
The related system of people or ectoskeleton has highly sensitive and stability.
A kind of Torque Control system of power joint arrangement provided in this embodiment, including signal acquisition unit and signal
Processing unit, wherein signal acquisition unit includes the rotary encoder 16 connecting with the motor 13 of driving joint 1 movement, measurement
The torque sensor 15 of 12 opposing torque of upper arm 11 and lower arm, the angular transducer between 1 upper arm 11 of joint and lower arm 12
17 and the current sensor 21 that connect of motor 13 that is moved with driving joint 1；Wherein, rotary encoder 16 acquires motor 13
Rotational angle θ m, torque sensor 15 acquire the opposing torque Ts between upper arm 11 and lower arm 12, in the acquisition of angular transducer 17
The relative angle θ l rotated between arm 11 and lower arm 12, current sensor 21 acquire electric current Ifbk when motor 13 works；Signal
Processing unit includes memory 23 and processor 22, is equipped with torque ring controller 24 and current loop controller in processor 22
25, the output of torque ring controller 24 is the control target Tm of current loop controller 25；
And Tm=A* (de-F (S))+B*T_hat+Z_hat；
Wherein, A, B are the constant coefficient determined according to system model；De is error derivative value, and error e is equal to Tref-Ts
Or its filtered data, Tref are the control target of torque ring controller 24；F (S) is synovial membrane Reaching Law function, wherein S=Cs*
De+e, Cs are adjustability coefficients；T_hat is Ts or its data after being filtered；Z_hat is system interference data, system interference
Data Z_hat includes load and the friction interference value z23 of 13 frictional force interference value z13 of motor and/or upper arm 11 and lower arm 12, is
System interference data Z_hat meets C*z13 or D*z23 or the sum of C*z1 and D*z23, C=-Jm, D=N*Jm, and Jm is motor
13 rotary inertia, N are the 14 structure reduction ratio of speed reducer in power joint 1.
For signal acquisition unit, mainly it is arranged on 1 device of power joint, wherein referring to Fig.1,1 device of power joint
It include upper arm 11, lower arm 12, motor 13 and speed reducer 14, upper arm 11 and lower arm 12 are in motor 13 and the work of speed reducer 14
Under, it can relatively rotate, upper arm 11 and lower arm 12 can have relative angle and opposing torque during relative rotation,
Therefore, the opposing torque Ts between upper arm 11 and lower arm 12 is measured using torque sensor 15, angular transducer 17 acquires upper arm
The relative angle θ l rotated between 11 and lower arm 12, in addition, 1 device of power joint is equipped with motor 13 for motor 13
Driving circuit is rotated for driving motor 13, since there are frictional force, the electric currents of 13 driving circuit of motor output inside motor 13
Electric current when motor 13 works can be greater than, current sensor 21 acquires electric current Ifbk when motor 13 works, that is, motor 13
Actual current, one of the data to make compensation as subsequent torque ring controller 24.
Processor 22 is placed with the instruction for realizing every filtering and observation, can issue control instruction control motor 13 according to
The requirement output torque of instruction, in general, program corresponding to control instruction can be stored in memory 23, in practice
In, processor 22 can transfer in real time the program in memory 23 and execute the process of signal processing.
For above-mentioned mentioned Tm=A* (de-F (S))+B*T_hat+Z_hat, and referring to Fig. 2.
Within the set time, give the motor 13 of 1 device of power joint certain electric current, so that 1 device of power joint is opened
Beginning to operate, rotary encoder 16, torque sensor 15, angular transducer 17 and current sensor 21 acquire corresponding data,
Target Tref, that is, the torque value that torque ring controller 24 theoretically inputs are controlled in conjunction with torque ring controller 24, by data
And control target Tref be input in torque ring controller 24 be filtered, differential, observation and controller, calculate electric current
The control target Tm of ring controller 25, that is, the input torque value that current loop controller 25 should theoretically have, electric current loop
The data of control target Tm and current sensor 21 acquisition of controller 25 can make using output after current loop controller 25
1 device of power joint reaches highly sensitive electric current, and is input to motor 13, and the torque of approach Tm is exported by motor 13, to drive
Dynamic 1 device of power joint, which is given, to react.
In other words, the workflow of whole system is: utilizing rotary encoder 16, torque sensor 15, angle sensor
The field evidence for 1 device of power joint that device 17 and current sensor 21 acquire, controls in conjunction with torque ring controller 24
Target Tref obtains the control target Tm of current loop controller 25, so that field evidence levels off to satisfactory data,
Achieve the effect that closed-loop control, feedback compensation is had, to reach the sensitivity for improving entire 1 device of power joint and Gao Lu
The effect of stick control.
Further, above-mentioned constant coefficient A, B meet:
A=N*Jm/Cs；
B=1/N+N*Jm/Jl；
Wherein, Jl is the rotary inertia in power joint 1 and load, and Cs is adjustability coefficients.
Further, referring to Fig. 3, the input data of above-mentioned torque ring controller 24 includes rotary encoder 16, turns round
Square sensor 15, angular transducer 17 and current sensor 21 acquire corresponding data and torque ring controller 24 controls mesh
Tref is marked, output connection current loop controller 25, the output of current loop controller 25 connects motor 13, specifically, above-mentioned
It is equipped with processing module 243, differential filter 241 and expansion observer 242 in torque ring controller 24, passes through differential filter
The 241 or expansion observation acquisition error derivative value de of observer 242, passes through expansion observer 242 and obtains system interference data Z_
The data that hat, the differential filter 241 and expansion observer 242 obtain, are input to the processing module 243, are located
The control target Tm of reason output current loop controller 25.
Further, above-mentioned expansion observer 242 include first expansion observer 2421 and at least one second
Observer 2422 is expanded, the first expansion observer 2421 is observed the rotational angle θ m of motor 13 or its filter value, exports
13 frictional force interference value z13 of motor, the second expansion observer 2422 export load and the friction interference value of upper arm 11 and lower arm 12
z23。
Wherein, the first expansion observer 2421 meets the following conditions:
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)；
The wherein derivative that dz11, dz12, dz13 respectively correspond as z11, z12, z13, β 10, β 11, β 12, α 11, α 12, δ
11, δ 12 is adjustable parameter, and b11, b12 are the constant according to system under test (SUT) parameter setting, and one of which is selected as b11=1/Jm,
B12=-1/ (Jm*N), fal are monotonic function.
The input variable of first expansion observer 2421 is θ m_hat, Ifbk_hat and Ts_hat, wherein θ m_hat is
Rotary encoder 16 acquires the rotational angle θ m or its filter value of motor 13, and Ifbk_hat is that current sensor 21 acquires motor 13
Electric current Ifbk or its filter value when work, Ts_hat are that torque sensor 15 acquires turning round between upper arm 11 and lower arm 12 relatively
Square Ts or its filter value, output variable z11, z12, z13, wherein z11 approaches rotational angle θ m, z12 the approach electricity of motor 13
The derivative value of the rotational angle θ m of machine 13, z13 level off to current motor 13 rub interference value.
The second expansion observer 242 meets the following conditions:
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)；
Wherein, the derivative that dz21, dz22, dz23 respectively correspond as z21, z22, z23, β 20, β 21, β 22, α 21, α 22, δ
21, δ 22 is adjustable parameter, and b22 is the constant according to system under test (SUT) parameter setting, and one of which is selected as b22=1/Jl, and Jl is
The rotary inertia of power joint 1 and load, fal is monotonic function.
The input variable of the second above-mentioned expansion observer 2422 is θ l_hat and Ts_hat, and θ l_hat is angular transducer
Relative angle θ l or its filter value between 17 acquisition upper arm 11 and lower arm 12, Ts_hat are that torque sensor 15 acquires upper arm 11
Opposing torque Ts or its filter value between lower arm 12, output variable z21, z22, z23, wherein z21 approach upper arm 11 with
Relative angle θ l, z22 between lower arm 12 approach the derivative value of the relative angle θ l between upper arm 11 and lower arm 12, z23 approach
Load and friction interference value in upper arm 11 and lower arm 12.
Further, above-mentioned error derivative value de meets de=dTref-dTs, wherein dTs=z32, dTref=
dz41。
In addition, above-mentioned expansion observer 242 includes that third expands observer 2423, wherein third expands observer
2423 output z32, third expansion observer 2423 meet the following conditions:
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)；
Wherein, the derivative that dz31, dz32, dz33 respectively correspond as z31, z32, z33, β 30, β 31, β 32, α 31, α 32, δ
31, δ 32 is adjustable parameter, and b31, b32 are the constant according to system under test (SUT) parameter setting, and one of which is selected as b31=K/ (N*
Jm), b32=K/Jl+K/ (N*N*Jm), K are the output end of motor 13 to the 1 load equivalent spring coefficient of stiffiness of joint, and Jl is
Power joint 1 and load rotating inertia, fal are monotonic function；B33, b34 are the constant according to system under test (SUT) parameter setting, wherein
One kind being selected as b33=K/N, b34=-K.
It is Ifbk_hat, Ts_hat that third, which expands 2423 input variable of observer, and Ifbk_hat is when motor 13 works
Electric current Ifbk or its filter value, opposing torque Ts or its filter value of the Ts_hat between upper arm 11 and lower arm 12, output variable
For z31, z32, z33, wherein z31 approaches Ts, and z32 approaches the derivative value of Ts.
In other embodiments, above-mentioned third expansion observer 2423 meets the following conditions:
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)；
Wherein, α 31, α 32, δ 31, δ 32 are adjustable parameter, and b33, b34 are the constant according to system under test (SUT) parameter setting,
Middle one kind is selected as b33=K/N, and b34=-K, K are the output end of motor 13 to the 1 load equivalent spring coefficient of stiffiness of joint, Jl
For power joint 1 and load rotating inertia, fal is monotonic function.
It is Ifbk_hat, Ts_hat that third, which expands 2423 input variable of observer, and Ifbk_hat is when motor 13 works
Electric current Ifbk or its filter value, opposing torque Ts or its filter value of the Ts_hat between upper arm 11 and lower arm 12, output variable
For z31, z32, z33, wherein z31 approaches Ts, and z32 approaches the derivative value of Ts.
Further, differential filter 241 includes the first differential filter 2411, wherein the first differential filter
2411 output z41；And first differential filter 2411 meet the following conditions:
Eq41:e4=z41-Tref；
Eq42:dz41=- β 40*fal (e4, α 42, δ 42)；
Wherein, dz41 is the derivative of z41, and β 40, α 42, δ 42 is adjustable parameter, and fal is monotonic function, the first differential filter
Control target Tref or its filter value of 2411 input variable of device for torque ring controller 24, output variable z41, dz41,
The control target Tref numerical value of middle z41 approach torque ring controller 24, dz41 approach the control target of torque ring controller 24
The derivative value dTref of Tref.
Further, above-mentioned differential filter 241 further includes the second differential filter 2412, third differential filter
2413, the 4th differential filter 2414, one or more 5th differential filters 2415；Second differential filter 2412, third
Differential filter 2413, the 4th differential filter 2414 and the 5th differential filter 2415 are respectively corresponded to upper arm 11 and lower arm
Relative angle θ l, the motor rotated between rotational angle θ m of opposing torque Ts, motor 13 between 12, upper arm 11 and lower arm 12
At least one in electric current Ifbk when 13 work is filtered and differential process；Wherein, the second above-mentioned differential filter 2412
Meet the following conditions:
Eq51:e5=z51-Ts；
Eq52:dz51=- β 50*fal (e5, α 52, δ 52)；
Wherein, dz51 is the derivative of z51, and β 50, α 52, δ 52 is adjustable parameter, and fal is monotonic function, and input variable is upper
Opposing torque Ts or its filter value between arm 11 and lower arm 12, output variable z51, dz51, wherein z51 approach upper arm 11 with
Opposing torque Ts numerical value between lower arm 12, dz51 approach the derivative value dTs of the opposing torque Ts between upper arm 11 and lower arm 12.
Above-mentioned third differential filter 2413 meets the following conditions:
Eq61:e6=z61- θ m；
Eq62:dz61=- β 60*fal (e6, α 62, δ 62)；
Wherein, dz61 is the derivative of z61, and β 60, α 62, δ 62 is adjustable parameter, and fal is monotonic function, and input variable is electricity
The rotational angle θ m or its filter value of machine 13, output variable are z61 and dz61, and z61 approaches the rotational angle θ m number of motor 13
Value, dz61 approach the derivative value d θ m of the rotational angle θ m of motor 13.
The 4th above-mentioned differential filter 2414 meets the following conditions:
Eq71:e7=z71- θ l；
Eq72:dz71=- β 70*fal (e7, α 72, δ 72)；
Wherein, dz71 is the derivative of z71, and β 70, α 72, δ 72 is adjustable parameter, and fal is monotonic function, and input variable is upper
The relative angle θ l or its filter value rotated between arm 11 and lower arm 12, output variable z71, dz71, wherein z71 approaches upper arm
The relative angle θ l numerical value rotated between 11 and lower arm 12, the relative angle θ l rotated between dz71 approach upper arm 11 and lower arm 12
Derivative value d θ l.
The 5th above-mentioned differential filter 2415 meets the following conditions:
Eq81:e8=z81-Ifbk；
Eq82:dz81=- β 80*fal (e8, α 82, δ 82)；
Wherein dz81 is the derivative of z81, and β 80, α 82, δ 82 is adjustable parameter, and fal is monotonic function, and input variable is electricity
Electric current Ifbk or its filter value when machine 13 works, output variable z81, dz81, wherein z81 approaches electricity when motor 13 works
Ifbk numerical value is flowed, dz81 approaches the derivative value dIfbk of electric current Ifbk when motor 13 works.
Further, above-mentioned synovial membrane Reaching Law function F (S) is constant speed Reaching Law function F=-sign (S) * ε, or
Exponentially approaching rule function F=-ks*S-sign (S) * ε or power Reaching Law function F=-ks*abs (S) ^alpha*sign
(S), wherein ε, ks, alpha are adjustable parameter, and sign is sign function, and abs is ABS function.
In the present embodiment, the output end of above-mentioned motor 13 is to 1 load equivalent spring coefficient of stiffiness K of joint, motor 13
Rotary inertia Jm, the 14 structure reduction ratio N of speed reducer in power joint 1, power joint 1 and load rotating inertia J1 numerical value
To measure by experiment, above-mentioned numerical value is obtained particular by the mode that multiple measurement obtains average value.
Above-mentioned monotonic function fal function is power function f1, or is arc tangent nonlinear function f2；
Wherein, the power function f1 expression formula are as follows:
The arc tangent nonlinear function f2 expression formula is as follows: f2 (e, α, β)=β * atan (2 α e/ π).
Further, the input variable of above-mentioned processing module 243 is z41, z51, dz41, z32, z13, z23, processing
The output variable of module 243 is Tm, and above-mentioned processing module 243 realizes that following four calculates:
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 are adjustable parameter, constant coefficient A=N*Jm/Cs, B=1/N+N*
Jm/Jl, C=-Jm, D=N*Jm, Jm are 13 rotary inertia of motor, and Jl is the relative rotation inertia of upper arm 11 and lower arm 12, and N is
The reduction ratio of speed reducer 14, Cs are adjustable parameter.
Current loop controller 25 realizes control using traditional PI ring, and 13 output torque of driving motor approaches torque ring controller
The control target Tm of 24 outputs.Control current loop controller 25 method be mainly PID control method or synovial membrane control method or
Method for optimally controlling.
A kind of Torque Control system of above-mentioned power joint arrangement is compiled by the way that rotation is arranged on 1 device of power joint
Code device 16, angular transducer 17, torque sensor 15 and current sensor 21, use synovial membrane to control in signal processing unit with
It realizes quick response, interfered using expansion 242 observation system of observer and obtains the high quality derivative of correlative observable, use is micro-
Filter-divider 241 is filtered to input data and obtains derivative, handles output current loop controller 25 using processing module 243
Control target, to control the output torque in joint 1, by input data and control target data be filtered, differential, sight
Survey and control calculate, with realize joint of robot 1 under the quick situation of change of flexible, frictional force, reach quick response,
The Torque Control of strong robustness so that the related system of robot or ectoskeleton with dynamic 1 device of joint have it is highly sensitive
Degree and stability.
As shown in figure 4, the present embodiment additionally provides a kind of Torque Control side of the Torque Control system of power joint arrangement
Method, this method comprises:
Opposing torque Ts, upper arm 11 and lower arm between S1, rotational angle θ m for obtaining motor 13, upper arm 11 and lower arm 12
The control target of electric current Ifbk and torque ring controller 24 when relative angle θ l, the motor 13 rotated between 12 works
Tref；
S2, observation or the filtering θ m, θ l, Ts obtain the output z13 of the first expansion observer 2421, the second expansion is seen
Survey the output of the output z23 of device 2422, the output z32 and the first differential filter 2411 of third expansion observer 2423
dz41；
S3, according to e=Tref-Ts, wherein e be actual measured value Ts with control target Tref deviation or approximation；de
=dz41-z32 or de=-z32, wherein de is the derivative approximation of deviation e；S=Cs*de+e, wherein S is synovial membrane face ginseng
Number, Cs is adjustability coefficients；Tm=A* (de-F (S))+B*Ts+C*z13+D*z23, wherein Tm is the output of torque ring controller 24
Value；Obtain the output Tm of torque ring controller 24；
S4, current loop controller 25 are control target with Tm, and control motor 13 exports control moment and approaches Tm, and returns
The S1 step stated.
For above-mentioned S1 step, the main rotational angle θ m that motor 13 is acquired by rotary encoder 16, torque sensing
Device 15 acquires the opposing torque Ts between upper arm 11 and lower arm 12, and angular transducer 17 acquires to be rotated between upper arm 11 and lower arm 12
Relative angle θ l, current sensor 21 acquire motor 13 work when electric current Ifbk, and out of control instruction obtain torque ring
The control target Tref of controller 24.
For above-mentioned S2 step, observer 2422 is expanded using the first expansion observer 2421, second, third expansion is seen
It surveys device 2423 and the first differential filter 2411 is observed or filters to input variable θ m, θ l, Ts, Tref respectively, wherein
Input variable θ m_hat=the θ m, the input variable θ l_hat=θ of the second expansion observer 2422 of first expansion observer 2421
L, third expand the input variable Ts_hat=Ts of observer 2423, and the input of the first differential filter 2411 is Tref, respectively
The output z13 of the first expansion observer 2421 is obtained, second expands the output z23 of observer 2422, and third expands observer
The 2423 output z32 and output dz41 of the first differential filter 2411, wherein z32 levels off to the derivative value of Ts, and z13 becomes
It is bordering on the friction interference value of motor 13, z23 levels off to the load of lower arm 12 and friction interference value.
Further, after S2 step, further includes:
S21, Tref, Ts, θ m, θ l, the Ifbk of input are observed respectively, obtain output data z41, dz41, z51,
dz51、z61、z71、z81。
Above-mentioned S21 step, mainly first is micro- using differential filter 241, the second differential filter 24122, third
Filter-divider 2413, the 4th differential filter 2414, the 5th differential filter 2415 are respectively to input variable Tref, Ts, θ m, θ
L, Ifbk is observed, and is respectively corresponded and is obtained output data z41, dz41, z51, dz51, z61, z71, z81.Specifically, Tref
Tracking signal be z41, the differential signal of Tref is dz41, and the tracking signal of Ts is z51, and the differential signal of Ts is dz51, θ m
Tracking signal be z61, the tracking signal of θ l is z71, and the tracking signal of Ifbk is z81；In other words, z41 approaches Tref,
Dz41 approaches Tref derivative value, and z51 approaches Ts, and z61 approaches θ m, and z71 approaches θ l, and z81 approaches Ifbk.
For above-mentioned S3 step, specifically, above-mentioned e=Tref (or z41)-Ts (or z31 or z51), wherein e is real
Border measured value Ts and the deviation for controlling target Tref；
De=dz41-z32 (or dz51) or de=-z32, wherein de is the derivative approximation of deviation e；
S=Cs*de+e, wherein S is synovial membrane face parameter, and Cs is adjustability coefficients；
Tm=A* (de-F (S))+B*Ts+C*z13+D*z23, wherein Tm is 24 output valve of torque ring controller.
Most preferably, 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) * ε；A, B, C, D be according to the constant coefficient of system under test (SUT) parameter setting,
A=N*Jm/Cs, B=1/N+N*Jm/Jl, C=-Jm, D=N*Jm, wherein Jm is 1 motor of power joint, 13 rotary inertia,
N is 1 speed reducer of power joint, 14 structure reduction ratio, and Jl is the power joint 1 and load rotating inertia, Jm, N, Jl numerical value
To be measured by experiment, ε, ks, Cs are adjustable parameter；
For above-mentioned S4 step, drive control mainly is implemented to motor 13 using Tm as target:
It is controlled by PI and calculates vector controlled input voltage value:
Ie=Ie+Tm*Km-Ifbk；
Pout=Kp* (Tm*Km-Ifbk)+Ki*Ie；
Input value is driven to control PWM output by motor 13 of Pout, wherein Km is constant coefficient, can test and measure.
In addition, for above-mentioned S4 step, in practical control process, using pid control mode or synovial membrane control
Mode or optimal control mode processed.
It opposite can be adjusted in the implementation sequence of other embodiments, above-mentioned S21 step and above-mentioned S2 step.
It is above-mentioned that technology contents of the invention are only further illustrated with embodiment, in order to which reader is easier to understand, but not
It represents embodiments of the present invention and is only limitted to this, any technology done according to the present invention extends or recreation, by of the invention
Protection.Protection scope of the present invention is subject to claims.
Claims (11)
1. a kind of Torque Control system of power joint arrangement, which is characterized in that including signal acquisition unit and signal processing
Unit, wherein signal acquisition unit includes the rotary encoder connecting with the motor of driving joint motions, measurement upper arm lower arm phase
To the torque sensor of torque, the angular transducer between joint upper arm and lower arm and the motor with driving joint motions
The current sensor of connection；Wherein, the rotational angle θ m of rotary encoder acquisition motor, torque sensor acquire upper arm and lower arm
Between opposing torque Ts, the relative angle θ l that rotates between angular transducer acquisition upper arm and lower arm, current sensor acquisition
Electric current Ifbk when motor works；Signal processing unit includes memory and processor, is equipped with torque ring in processor and controls
Device and current loop controller, the output of torque ring controller are the control target Tm of current loop controller；
And Tm=A* (de-F (S))+B*T_hat+Z_hat；
Wherein, A, B are the constant coefficient determined according to system model；De be error derivative value, error e be equal to Tref-Ts or its
Filtered data, Tref are the control target of torque ring controller；F (S) is synovial membrane Reaching Law function, wherein S=Cs*de+e,
Cs is adjustability coefficients；T_hat is Ts or its data after being filtered；Z_hat is system interference data, system interference data Z_
Hat includes load and friction the interference value z23, system interference data Z_ of motor friction interference value z13 and/or upper arm and lower arm
Hat is option, meets C*z13 or D*z23 or the sum of C*z1 and D*z23, C=-Jm, D=N*Jm, and Jm is motor
Rotary inertia, N are the deceleration mechanism reduction ratio in power joint.
2. a kind of Torque Control system of power joint arrangement according to claim 1, which is characterized in that the constant system
Number A, B meet:
A=N*Jm/Cs；
B=1/N+N*Jm/Jl；
Wherein, Jl is the rotary inertia of power joint and load, and Cs is adjustability coefficients.
3. a kind of Torque Control system of power joint arrangement according to claim 1, which is characterized in that the torque ring
It is equipped with processing module, differential filter and expansion observer in controller, is obtained by differential filter or expansion observer observation
Error derivative value de is obtained, system interference data Z_hat, the differential filter and expansion observation are obtained by expansion observer
The data that device obtains, are input to the processing module, carry out the control target Tm of processing output current loop controller.
4. a kind of Torque Control system of power joint arrangement according to claim 3, which is characterized in that the expansion is seen
Surveying device includes that the first expansion observer and at least one second expansion observer, the first expansion observer turn motor
Dynamic angle, θ m or its filter value are observed, and output motor frictional force interference value z13, the second expansion observer exports upper arm
Load and friction interference value z23 with lower arm；Wherein, the first expansion observer meets the following conditions:
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)；
The wherein derivative that dz11, dz12, dz13 respectively correspond as z11, z12, z13, β 10, β 11, β 12, α 11, α 12, δ 11, δ 12
For adjustable parameter, b11, b12 are the constant according to system under test (SUT) parameter setting, and one of which is selected as b11=1/Jm, b12=-
1/ (Jm*N), fal are monotonic function；
The input variable of the first expansion observer is θ m_hat, Ifbk_hat and Ts_hat, wherein θ m_hat is rotation
Encoder acquires the rotational angle θ m or its filter value of motor, and Ifbk_hat is electric current when current sensor acquires motor work
Ifbk or its filter value, Ts_hat is the opposing torque Ts or its filter value between torque sensor acquisition upper arm and lower arm, defeated
Variable is z11, z12, z13 out, wherein z11 approaches leading for the rotational angle θ m of rotational angle θ m, z12 the approach motor of motor
Numerical value, z13 level off to current motor friction interference value；
The second expansion observer meets the following conditions:
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 wherein derivative that dz21, dz22, dz23 respectively correspond as z21, z22, z23, β 20, β 21, β 22, α 21, α 22, δ 21, δ 22
For adjustable parameter, b22 is the constant according to system under test (SUT) parameter setting, and one of which is selected as b22=1/Jl, and Jl is power pass
Section and the rotary inertia of load, fal is monotonic function；
It is described second expansion observer input variable be θ l_hat and Ts_hat, θ l_hat be angular transducer acquire upper arm with
Relative angle θ l or its filter value between lower arm, Ts_hat are the opposing torque between torque sensor acquisition upper arm and lower arm
Ts or its filter value, output variable z21, z22, z23, wherein z21 approaches the relative angle θ l, z22 between upper arm and lower arm
Approach the derivative value of the relative angle θ l between upper arm and lower arm, z23 levels off to load and the friction interference value of upper arm and lower arm.
5. a kind of Torque Control system of power joint arrangement according to claim 3, which is characterized in that the error is led
Numerical value de meets de=dTref-dTs, wherein dTs=z32, dTref=dz41；
The expansion observer includes third expansion observer, wherein the third expansion observer exports z32, the third
Expansion observer meets the following conditions:
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*Ts+b33*z13+b34*z23；
Eq34:dz33=- β 32*fal (e3, α 32, δ 32)；
The wherein derivative that dz31, dz32, dz33 respectively correspond as z31, z32, z33, β 30, β 31, β 32, α 31, α 32, δ 31, δ 32
For adjustable parameter, b31, b32 are the constant according to system under test (SUT) parameter setting, and one of which is selected as b31=K/ (N*Jm),
B32=K/Jl+K/ (N*N*Jm), K are that the output end of motor arrives the joint loads equivalent spring coefficient of stiffiness, Jl for power joint and
Load rotating inertia, fal are monotonic function；B33, b34 are the constant according to system under test (SUT) parameter setting, and one of which is selected as
B33=K/N, b34=-K；
Third expand observer input variable be Ifbk_hat, Ts_hat, Ifbk_hat be motor work when electric current Ifbk or
Its filter value, Ts_hat the opposing torque Ts or its filter value between upper arm and lower arm, output variable z31, z32, z33,
Wherein z31 approaches Ts, and z32 approaches the derivative value of Ts.
6. a kind of Torque Control system of power joint arrangement according to claim 3, the differential filter includes the
One differential filter, wherein first differential filter exports z41；And first differential filter meets the following conditions:
Eq41:e4=z41-Tref；
Eq42:dz41=- β 40*fal (e4, α 42, δ 42)；
Wherein, dz41 is the derivative of z41, and β 40, α 42, δ 42 is adjustable parameter, and fal is monotonic function, and the first differential filter is defeated
Enter the control target Tref or its filter value that variable is torque ring controller, output variable z41, dz41, wherein z41 approaches power
The control target Tref numerical value of square ring controller, dz41 approach the derivative value dTref of the control target Tref of torque ring controller.
7. a kind of Torque Control system of power joint arrangement according to claim 3, which is characterized in that the differential filter
Wave device further includes the second differential filter, third differential filter, the 4th differential filter, one or more 5th differential filters
Device；Second differential filter, third differential filter, the 4th differential filter and the 5th differential filter respectively correspond
To the relative angle θ l rotated between rotational angle θ m, upper arm and lower arm of opposing torque Ts, motor between upper arm and lower arm,
At least one in electric current Ifbk when motor works is filtered and differential process；Wherein, second differential filter meets
The following conditions:
Eq51:e5=z51-Ts；
Eq52:dz51=- β 50*fal (e5, α 52, δ 52)；
Wherein, dz51 is the derivative of z51, and β 50, α 52, δ 52 is adjustable parameter, and fal is monotonic function, input variable be upper arm with
Opposing torque Ts or its filter value between lower arm, output variable z51, dz51, wherein z51 is approached between upper arm and lower arm
Opposing torque Ts numerical value, dz51 approach the derivative value dTs of the opposing torque Ts between upper arm and lower arm；
Third differential filter meets the following conditions:
Eq61:e6=z61- θ m；
Eq62:dz61=- β 60*fal (e6, α 62, δ 62)；
Wherein, dz61 is the derivative of z61, and β 60, α 62, δ 62 is adjustable parameter, and fal is monotonic function, and input variable is motor
Rotational angle θ m or its filter value, output variable are z61 and dz61, and z61 approaches the rotational angle θ m numerical value of motor, and dz61 becomes
The derivative value d θ m of the rotational angle θ m of nearly motor；
4th differential filter meets the following conditions:
Eq71:e7=z71- θ l；
Eq72:dz71=- β 70*fal (e7, α 72, δ 72)；
Wherein, dz71 is the derivative of z71, and β 70, α 72, δ 72 is adjustable parameter, and fal is monotonic function, input variable be upper arm with
The relative angle θ l or its filter value rotated between lower arm, output variable z71, dz71, wherein z71 approach upper arm and lower arm it
Between the relative angle θ l numerical value that rotates, the derivative value d θ l of the relative angle θ l rotated between dz71 approach upper arm and lower arm；
5th differential filter meets the following conditions:
Eq81:e8=z81-Ifbk；
Eq82:dz81=- β 80*fal (e8, α 82, δ 82)；
Wherein dz81 is the derivative of z81, and β 80, α 82, δ 82 is adjustable parameter, and fal is monotonic function, and input variable is motor work
Electric current Ifbk or its filter value when making, output variable z81, dz81, wherein z81 approaches electric current Ifbk number when motor works
Value, dz81 approach the derivative value dIfbk of electric current Ifbk when motor works.
8. a kind of Torque Control system of power joint arrangement according to any one of claims 1 to 7, which is characterized in that
The synovial membrane Reaching Law function F (S) is constant speed Reaching Law function F=-sign (S) * ε or exponentially approaching rule function F=-ks*
S-sign (S) * ε or power Reaching Law function F=-ks*abs (S) ^alpha*sign (S), wherein ε, ks, alpha be can
Parameter is adjusted, sign is sign function, and abs is ABS function.
9. according to a kind of described in any item Torque Control systems of power joint arrangement of claim 4 to 7, which is characterized in that
The monotonic function fal function is power function f1, or is arc tangent nonlinear function f2；
Wherein, the power function f1 expression formula are as follows:
The arc tangent nonlinear function f2 expression formula is as follows: f2 (e, α, β)=β * atan (2 α e/ π).
10. a kind of Torque Control method of the Torque Control system of power joint arrangement, which is characterized in that the described method includes:
Obtain rotated between the opposing torque Ts between rotational angle θ m of motor, upper arm and lower arm, upper arm and lower arm it is opposite
The control target Tref of electric current Ifbk and torque ring controller when angle, θ l, motor work；
The θ m, θ l, Ts are observed or filtered, the output of the output z13, the second expansion observer of the first expansion observer are obtained
The output dz41 of z23, the output z32 of third expansion observer and the first differential filter；
According to e=Tref-Ts, wherein e is actual measured value Ts and controls the deviation or approximation of target Tref；De=dz41-
Z32 or de=-z32, wherein de is the derivative approximation of deviation e；S=Cs*de+e, wherein S is synovial membrane face parameter, and Cs is
Adjustability coefficients；Tm=A* (de-F (S))+B*Ts+C*z13+D*z23, wherein Tm is torque ring controller output valve；Obtain torque
The output Tm of ring controller；
Current loop controller is control target with Tm, and control motor output control moment approaches Tm, and returns to the acquisition motor
Rotational angle θ m, the opposing torque Ts between upper arm and lower arm, the relative angle θ l rotated between upper arm and lower arm, motor work
The step of control target Tref of electric current Ifbk and torque ring controller when making.
11. a kind of Torque Control method of the Torque Control system of power joint arrangement according to claim 10, special
Sign is, observes or filter the θ m, θ l, Ts, and the output z13 of acquisition the first expansion observer, second expand the defeated of observer
Out after the step of output dz41 of z23, the output z32 of third expansion observer and the first differential filter further include:
Tref, Ts, θ m, θ l, the Ifbk of input are observed respectively, obtain output data z41, dz41, z51, dz51, z61,
z71、z81。
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 true CN109108954A (en) | 2019-01-01 |
| CN109108954B 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) |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110027002A (en) * | 2019-04-24 | 2019-07-19 | 深圳市智能机器人研究院 | A kind of bionic joint control system and method based on multi-motor driving |
| CN112809651A (en) * | 2019-11-15 | 2021-05-18 | 深圳市英汉思动力科技有限公司 | Power-assisted exoskeleton control method, power-assisted exoskeleton control system and computer equipment |
| WO2021184517A1 (en) * | 2020-03-17 | 2021-09-23 | 北京理工大学 | Robot joint torque control system and load compensation method therefor |
| CN113891786A (en) * | 2019-04-17 | 2022-01-04 | 优傲机器人公司 | Method for controlling robot arm based on adaptive friction |
| CN114533488A (en) * | 2022-02-18 | 2022-05-27 | 山东泽普医疗科技有限公司 | 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 |
Cited By (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113891786A (en) * | 2019-04-17 | 2022-01-04 | 优傲机器人公司 | Method for controlling robot arm based on adaptive friction |
| CN110027002A (en) * | 2019-04-24 | 2019-07-19 | 深圳市智能机器人研究院 | A kind of bionic joint control system and method based on multi-motor driving |
| CN110027002B (en) * | 2019-04-24 | 2021-09-17 | 深圳市智能机器人研究院 | Multi-motor drive based bionic joint control system and method |
| CN112809651A (en) * | 2019-11-15 | 2021-05-18 | 深圳市英汉思动力科技有限公司 | Power-assisted exoskeleton control method, power-assisted exoskeleton control system and computer equipment |
| CN112809651B (en) * | 2019-11-15 | 2021-10-29 | 深圳市英汉思动力科技有限公司 | Power-assisted exoskeleton control method, power-assisted exoskeleton control system and computer equipment |
| WO2021184517A1 (en) * | 2020-03-17 | 2021-09-23 | 北京理工大学 | Robot joint torque control system and load compensation method therefor |
| CN114533488A (en) * | 2022-02-18 | 2022-05-27 | 山东泽普医疗科技有限公司 | Multi-joint constant-speed training control method and system |
| CN114533488B (en) * | 2022-02-18 | 2023-09-26 | 山东泽普医疗科技有限公司 | Multi-joint constant speed training control method and system |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109108954B (en) | 2020-07-21 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109108954A (en) | A kind of the Torque Control system and its Torque Control method of power joint arrangement | |
| CN109676607B (en) | Zero gravity control method without torque sensing | |
| CN106426174B (en) | A kind of robotic contact power detection method based on torque observation and Friction identification | |
| CN109358506B (en) | Self-adaptive fuzzy teleoperation control method based on disturbance observer | |
| Loffler et al. | Sensors and control concept of a biped robot | |
| CN108656112A (en) | A kind of mechanical arm zero-force control experimental system towards direct teaching | |
| CN109940622A (en) | It is a kind of based on the robot arm of current of electric without sensing collision checking method | |
| CN106842905B (en) | PID control method and device | |
| CN104062983B (en) | A kind of position control system with high accuracy of harmonic drive and method | |
| CN106873383A (en) | A kind of On-Line Control Method for reducing industrial robot vibration | |
| Zhang et al. | Output feedback control for free-floating space robotic manipulators base on adaptive fuzzy neural network | |
| Dong et al. | Decentralized control of harmonic drive based modular robot manipulator using only position measurements: theory and experimental verification | |
| CN109202873A (en) | A kind of flexible mechanical arm and its control method of cooperating | |
| CN102117081A (en) | Tension control module arranged in transducer and provided with tension and speed close loop | |
| Bascetta et al. | End-point vibration sensing of planar flexible manipulators through visual servoing | |
| Liu et al. | Robot manipulator disturbance observation and servo-fault diagnosis without speed sensor | |
| Wang et al. | Continuous finite-time control approach for series elastic actuator | |
| CN106855466B (en) | A kind of big flexible load simulator of single-degree-of-freedom | |
| Wang et al. | Nonlinear disturbance observer based torque control for series elastic actuator | |
| Prüfer et al. | Friction analysis and modelling for geared robots | |
| CN103170976A (en) | Two-freedom-degree robot motion control method | |
| Silawatchananai et al. | Force control of an upper limb exoskeleton for virtual reality using impedance control | |
| Ohashi et al. | Nonlinear disturbance compensation for haptic device | |
| EP2890611B1 (en) | Automatic system of stabilisation of suspended loads with automatic control | |
| Shi et al. | Observer-based finite time robust stabilization of mechanical arm systems |
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 |