CN108189036A - Torque control method and device, robot and storage medium - Google Patents
Torque control method and device, robot and storage medium Download PDFInfo
- Publication number
- CN108189036A CN108189036A CN201810045231.2A CN201810045231A CN108189036A CN 108189036 A CN108189036 A CN 108189036A CN 201810045231 A CN201810045231 A CN 201810045231A CN 108189036 A CN108189036 A CN 108189036A
- Authority
- CN
- China
- Prior art keywords
- motion
- torque control
- control
- torque
- parameter
- 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
- 238000000034 method Methods 0.000 title claims abstract description 90
- 230000033001 locomotion Effects 0.000 claims abstract description 237
- 230000001133 acceleration Effects 0.000 claims abstract description 63
- 230000008569 process Effects 0.000 claims abstract description 30
- 239000011159 matrix material Substances 0.000 claims description 53
- 230000015654 memory Effects 0.000 claims description 21
- 239000013598 vector Substances 0.000 claims description 21
- 238000004364 calculation method Methods 0.000 claims description 12
- 230000005484 gravity Effects 0.000 claims description 9
- 238000001914 filtration Methods 0.000 claims description 7
- 241000208340 Araliaceae Species 0.000 claims description 4
- 235000005035 Panax pseudoginseng ssp. pseudoginseng Nutrition 0.000 claims description 4
- 235000003140 Panax quinquefolius Nutrition 0.000 claims description 4
- 235000008434 ginseng Nutrition 0.000 claims description 4
- 230000006870 function Effects 0.000 description 19
- 238000010586 diagram Methods 0.000 description 10
- 238000005070 sampling Methods 0.000 description 7
- 238000005516 engineering process Methods 0.000 description 5
- 230000000694 effects Effects 0.000 description 4
- 230000005540 biological transmission Effects 0.000 description 3
- 238000004891 communication Methods 0.000 description 3
- 230000014509 gene expression Effects 0.000 description 3
- 230000010354 integration Effects 0.000 description 3
- 230000008901 benefit Effects 0.000 description 2
- 238000010276 construction Methods 0.000 description 2
- 239000000463 material Substances 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 1
- 241000406668 Loxodonta cyclotis Species 0.000 description 1
- 230000009471 action Effects 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
- 210000000707 wrist Anatomy 0.000 description 1
Classifications
-
- 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
-
- 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/1602—Programme controls characterised by the control system, structure, architecture
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Feedback Control In General (AREA)
Abstract
The invention discloses a torque control method, a torque control device, a robot and a storage medium. The torque control method comprises the following steps: acquiring a position error set, a speed error set and an expected running acceleration set of a motion control object at the current moment in the running process; determining a moment control parameter set according to the position error set and the speed error set, wherein the moment control parameter set comprises a robust compensation parameter set and a closed-loop feedback parameter set; and controlling the driving torque of the motion control object according to the torque control parameter set and the expected running acceleration set. By adopting the torque control method, the technical problem that the accuracy of the driving torque output by the mechanical arm is low due to the fact that dynamic errors cannot be adjusted in the prior art can be solved.
Description
Technical field
The present invention relates to technical field of robot control more particularly to a kind of Torque Control method, apparatus, robot and deposit
Storage media.
Background technology
Mechanical arm is a kind of Mechatronic device for simulating human arm, wrist and hand function.It can be by any object
Or spatially the time-varying of pose (position and posture) requires to be moved tool, it will so as to complete a certain industrial operation
It asks.The joint and connecting rod that typical mechanical arm is connected in series by some form, and each joint has one degree of freedom, Ke Yiping
It moves or rotates.
In general, mechanical arm is during the work time, tracing control can be carried out to the moving process of mechanical arm to ensure machine
The accuracy of tool arm movement.Wherein, when carrying out tracing control to mechanical arm, mechanical arm is weighed by static error and dynamic error
The stability of control system.Static error refers to the difference of the stable state output quantity of desired stable state output quantity and reality.Dynamic error
It is the function using the time as variable, the rule that control error changes over time when can provide system as stable state.However, the prior art
It when carrying out tracing control to mechanical arm, only can guarantee that the steady-state error of position tracking is smaller, and dynamic error can not be adjusted
It is whole, cause dynamic tracking accuracy low, and then can not ensure that mechanical arm exports accurate driving moment.
Invention content
In view of this, the embodiment of the present invention provides a kind of Torque Control method, apparatus, robot and storage medium, with solution
The technical issues of driving moment accuracy that mechanical arm is caused to export is low can not be certainly adjusted to dynamic error in the prior art.
In a first aspect, an embodiment of the present invention provides a kind of Torque Control method, including：
Obtain the site error set at current time, velocity error set and the expectation in the process of running of motion control object
Run acceleration set；
Torque Control parameter sets, the torque control are determined according to the site error set and the velocity error set
Parameter sets processed include robust compensation parameter sets and closed loop feedback parameter sets；
Acceleration set is run according to the Torque Control parameter sets and the expectation and controls the motion control object
Driving moment.
Second aspect, the embodiment of the present invention additionally provide a kind of moment control device, including：
Parameter acquisition module, for obtain motion control object in the process of running the site error set at current time,
Velocity error set and expectation run acceleration set；
Parameter determination module, for determining that Torque Control is joined according to the site error set and the velocity error set
Manifold is closed, and the Torque Control parameter sets include robust compensation parameter sets and closed loop feedback parameter sets；
Torque Control module, for running the control of acceleration set according to the Torque Control parameter sets and the expectation
The driving moment of the motion control object.
The third aspect, the embodiment of the present invention additionally provide a kind of robot, including：
One or more processors；
Memory, for storing one or more programs；
When one or more of programs are performed by one or more of processors so that one or more of processing
Device realizes Torque Control method as described in the present embodiment of the invention.
Fourth aspect, the embodiment of the present invention additionally provides a kind of storage medium for including computer executable instructions, described
Torque Control side of the computer executable instructions when being performed by computer processor for execution as described in the present embodiment of the invention
Method.
Torque Control method, apparatus, robot and the storage medium of above-mentioned offer, were being run by motion control object
Site error set, the velocity error set obtained in journey determines Torque Control parameter sets, and according to Torque Control parameter set
The technical solution of the driving moment of expectation acceleration set controlled motion control object closed and obtained in operational process so that most
The driving moment obtained eventually is more accurate, the accuracy of dynamic tracking when ensure that double-closed-loop control, while avoids control system
The external disturbance of system.
Description of the drawings
By reading the detailed description made to non-limiting example made with reference to the following drawings, of the invention is other
Feature, objects and advantages will become more apparent upon：
Fig. 1 a are the flow chart of a kind of Torque Control method that the embodiment of the present invention one provides；
Fig. 1 b are the robot architecture's schematic diagram for being equipped with motion control object；
Fig. 2 a are a kind of flow chart of Torque Control method provided by Embodiment 2 of the present invention；
Fig. 2 b are the flow chart of closed loop control method；
Fig. 2 c are data interaction structure diagram；
Fig. 2 d are the algorithm schematic diagram of Torque Control method；
Fig. 3 is the structure diagram of a kind of moment control device that the embodiment of the present invention three provides；
Fig. 4 is the structure diagram of a kind of robot that the embodiment of the present invention four provides.
Specific embodiment
The present invention is described in further detail with reference to the accompanying drawings and examples.It is understood that this place is retouched
The specific embodiment stated is used only for explaining the present invention rather than limitation of the invention.It also should be noted that in order to just
Part related to the present invention rather than full content are illustrated only in description, attached drawing.
Embodiment one
Fig. 1 a are the flow chart of a kind of Torque Control method that the embodiment of the present invention one provides.Torque provided in this embodiment
The situation of driving moment of the control method suitable for determining motion control object motion control object operation process.This implementation
The Torque Control method that example provides can be performed by moment control device, which can be by software and/or hard
The mode of part is realized, and is integrated in the robot for being equipped with motion control object.Wherein, robot refers to can be performed automatically
The installations of work.It can not only receive mankind commander, but also can run the program of advance layout, can also be according to artificial
The principle guiding principle action that intellectual technology is formulated.For example, shift fork, which lifts vehicle and the equipment with mechanical arm etc., belongs to robot.
With reference to figure 1a, a kind of Torque Control method provided in this embodiment specifically includes：
S110, motion control object the site error set at current time, velocity error set in the process of running are obtained
Acceleration set is run with expectation.
In the present embodiment, motion control object includes at least one moveable motion control subobject, each to move
Control subobject is each equipped with corresponding driving motor.By motion control object for for mechanical arm, corresponding motion control is sub
Object is made of a joint and corresponding connecting rod, and each joint is configured a driving motor and it is controlled to translate or revolve
Turn.Involved in the present embodiment to position, speed, acceleration may be considered the operation angle of corresponding motor, operation angle
Speed and operation acceleration.Fig. 1 b are the robot architecture's schematic diagram for being equipped with motion control object, wherein, motion control pair
As that can be mechanical arm, then the robot includes host computer 11, driver 12 and mechanical arm body 13.Wherein, host computer
11 can specifically include at least one processor and memory, for performing Torque Control method provided in this embodiment, drive
Dynamic device 12 and mechanical arm body 13 may be collectively referred to as motion control device.Typically, host computer 11 is equipped with real-time Linux operation
System, and can be communicated by network interface using real-time industrial field bus protocol with driver 12.Real-time industrial scene is total
Wire protocol can include：High level communication association of the framework on control area net(CAN) road (Controller Area Network, CAN)
Agreement (also referred to as CANOpen) or Ethernet auto-control technology (also referred to as EtherCAT) agreement etc..Driver 12 with
11 bus of host computer connects, and the driving moment control machinery arm body 13 for being determined according to host computer 11 moves, driver 12
With mechanical arm body 13 can be that bus connect can also be electrical connection, in Fig. 1 b by taking electrical connection as an example.In general, it drives
12 control machinery arm body 13 movement of dynamic device refers to the motor of corresponding motion control subobject is controlled to be operated.Optionally,
Mechanical arm body 13 further includes encoder (not shown), which can read the ginsengs such as running position and the speed of motor
Number, and the coded data of generation is sent to driver 12.Driver 12 measures the reading of encoder and is converted, and will convert
Result afterwards is by bus feedback to host computer 11.In general, what is be related to during following embodiments is obtained by measuring
Actual operation parameters can be understood as the data obtained by encoder measurement.It should be noted that above-mentioned robot is only
For illustrating, and the restriction not to the robot that motion control object is equipped in the present embodiment.
It should be noted that it is right to be each worth corresponding motion control in the set being related in the present embodiment
As, and in each set the corresponding value of same motion control subobject position it is identical.For example, first value in site error set
Same motion control subobject corresponding with first value in velocity error set, second value and speed are missed in site error set
The corresponding another motion control subobject of second value in difference set, and so on.In general, motion control subobject is carried out
Number, and determine putting in order for numerical value in each set according to number order.
Typically, each numerical value represents it is expected to reach under corresponding motion control subobject current time in site error set
To running position and the difference of running position that actually reaches.Each numerical value represents corresponding movement control in velocity error set
System it is expected the speed of service reached and the Operating speed difference actually reached under object current time.It is expected to run acceleration collection
Each numerical value represents it is expected the acceleration reached under corresponding control subobject current time in conjunction.Wherein, current time, which refers to, works as
The preceding time of running, it is mobile that operational process refers to the process of that controlled motion control object carries out.
Optionally, in motion control object initial launch, motion planning is carried out to motion control object.Wherein, it moves
It is planned to determine in this motion process of motion control object that each time of running it is expected the expectation reached operation in the ideal situation
Parameter sets.Wherein, which includes：It is expected speed of service set, it is expected running position set, it is expected fortune
At least one of row acceleration set.Refer in general, carrying out motion planning to motion control object to fortune control control pair
Each motion control subobject carries out motion planning as in, and the motion planning result of each motion control subobject is formed the phase
Hope operating parameter set.Specifically, existing motion planning may be used carries out motion planning to motion control object.
Further, host computer obtains the actual motion location sets for the current time motion control object that driver is sent
With actual motion sets of speeds.Determine that current time motion control object it is expected the expectation reached according to motion planning result simultaneously
Running position set it is expected speed of service set and it is expected to run acceleration set.According to actual motion location sets and phase
Running position set is hoped to determine the site error set at current time, according to actual motion sets of speeds and it is expected speed of service collection
Close the velocity error set for determining current time.
S120, Torque Control parameter sets are determined according to site error set and velocity error set.
Illustratively, when can determine that each motion control subobject is current according to site error set and velocity error set
The difference degree of desired operating status and actual motion state is carved, and then determines that each motion control is right according to the difference degree
The Torque Control parameter of elephant, obtains Torque Control parameter sets, to control corresponding motion control according to each moment control parameter
The driving moment of subobject, and then ensure motion control object running precision.
Wherein, Torque Control parameter sets include robust compensation parameter sets and closed loop feedback parameter sets.
Specifically, robustness be commonly defined as in the actual environment for ensure safety requirements, to motion control object into
During row operation control, control system minimum needs the requirement met.Robust compensation parameter sets be under current error parameter,
In order to meet motion control object robustness and the determining corresponding control parameter set of each motion control subobject.According to robust
When compensating parameter set is controlled, it can effectively ensure the robustness of motion control object, caused by avoiding external disturbance
It influences.Can be according to setting to site error set and velocity error set wherein it is determined that during robust compensation parameter sets
Computation rule is calculated.
Further, closed loop feedback parameter sets refer to what is determined according to site error set and velocity error set, use
In the control parameter set for correcting current operating parameter.Parameter sets can realize position-force control and speed based on closed-loop feedback
Spend closed-loop control so that the operational process of motion control object is more accurate.Wherein, closed loop feedback parameter sets can be by right
The mode that site error set and velocity error set carry out proportional plus integral control determines.
S130, the driving force that acceleration set controlled motion control object is run according to Torque Control parameter sets and expectation
Square.
Illustratively, driving moment refers to the torque of current time drive motion control object operation.In general, driving
Torque is matrix form, wherein, the parameter in the matrix on each diagonal corresponds to the driving of a motion control subobject
Torque.
In general, driving moment is by the inertia matrix of motion control subobject, coriolis force matrix, gravity matrix and rubs
It wipes torque battle array to determine, is specially that inertia matrix, coriolis force matrix, gravity matrix and frictional force matrix are subjected to addition meter
It calculates, and using obtained result of calculation as driving moment.Wherein, frictional force matrix is in each motion control subobject operational process
The set of joint-friction power.The dimension of above-mentioned parameter is related with the quantity of motion control subobject.In practical applications, according to
Above-mentioned parameter determines not considering external disturbance error during driving moment and dynamic tracking error is not adjusted, therefore, most
The driving moment obtained eventually is inaccurate, and when being moved according to the driving moment controlled motion control object, will be unable to ensure
The accuracy of dynamic tracking, i.e., cannot be such that motion control object is accurately moved.In view of this, it is proposed in the present embodiment
The driving moment of acceleration set controlled motion control object is run by Torque Control parameter sets and expectation.Due to determining power
Running position error and speed of service error are taken into account during square control parameter set, and is included in Torque Control parameter sets
Ensure the robust compensation parameter sets of robustness and the closed loop feedback parameter sets that tracing control determines in the process, and according to the phase
Hope operation acceleration set that can specify the acceleration that motion control object it is expected operation, therefore according to Torque Control parameter sets
During with it is expected to run acceleration set driving force square, it is believed that be to have fully taken into account current whole parameter type (positions
Put, speed, acceleration), and ensure that the robustness of system and the accuracy of double-closed-loop control.
Specifically, will respectively be worth in Torque Control parameter sets and it is expected that corresponding each value is added in operation acceleration set,
And it will add up result and represent in vector form.Further, which is multiplied with inertia matrix, the square that will be obtained after multiplication
Battle array is added to obtain driving moment matrix with coriolis force matrix, gravity matrix and frictional force matrix.Driving moment diagonal of a matrix
Output torque of the upper value for motion control object, wherein, the corresponding first motion control subobject of first value on diagonal
Output torque, the last one value corresponds to the output torque of the last one motion control subobject on diagonal.
Technical solution provided in this embodiment, the site error collection obtained in the process of running by motion control object
It closes, velocity error set determines Torque Control parameter sets, and according to being obtained in Torque Control parameter sets and operational process
It is expected the technical solution of the driving moment of acceleration set controlled motion control object so that the driving moment finally obtained is more
Accurately, the accuracy of dynamic tracking when ensure that double-closed-loop control, while avoid the external disturbance of control system.
Embodiment two
Fig. 2 a are a kind of flow chart of Torque Control method provided by Embodiment 2 of the present invention.The present embodiment is in above-mentioned reality
It applies and is embodied on the basis of example.With reference to figure 2a, Torque Control method provided in this embodiment specifically includes：
S210, the initial parameter set for obtaining the motion control object initial launch moment.
Specifically, initial parameter set includes：The initial position collection of initial launch moment motion control object actual motion
Conjunction, initial velocity set and initial acceleration set, further include：Ideally, initial launch moment motion control object
It is expected the desirable initial locations set reached, it is expected initial velocity set and it is expected initial acceleration set.Wherein, initial bit
Putting set, initial velocity set and initial acceleration can actually measure to obtain, and desirable initial locations set it is expected initial speed
Degree set and expectation initial acceleration set can be set according to actual conditions.Optionally, initial parameter collection is being obtained
During conjunction, the determining sampling period is synchronized, wherein, the sampling period is each parameter is sampled in motion control object operational process
Sampling interval.
S220, motion planning is carried out to motion control object according to initial parameter set, with true according to motion planning result
Determine current time corresponding expectation operating parameter set.
Specifically, to motion control object, this operational process carries out motion planning in advance, it can according to motion planning result
To determine expectation operating parameter set of the motion control object in each time of running, and then can be according to expectation operating parameter
Set determines that motion control object corresponds to moment actual motion with it is expected the difference degree of operation.
Illustratively, multi-motion planing method, this reality may be used when carrying out motion planning to motion control object
It applies in example, quintic algebra curve method has illustratively been selected to carry out motion planning.Fortune to being carried out based on quintic algebra curve method below
Dynamic planning carries out exemplary description：
In this example, wherein, the motion planning process of quintic algebra curve method can be expressed as：
S (t)=a0+a1t+a2t2+a3t3+a4t4+a5t5 (1)
Wherein, a0、a1、a2、a3、a4And a5To plan coefficient, the time of running current for motion control object t, S (t)
Motion planning result for t moment.It was found from above-mentioned formula, if it is desired to determine the motion planning of motion control object as a result,
The occurrence of clearly planning coefficient is needed, and plans that the occurrence of coefficient can be determined according to initial parameter set.Accordingly, it needs
The initial parameter set at motion control object initial launch moment is obtained, to determine planning coefficient according to initial parameter set.
Motion planning process is described by taking a motion control subobject as an example below.Wherein, in initial parameter collection
The corresponding initial parameter of current kinetic control subobject, setting sampling period T, initial position θ (0), initial velocity are searched in conjunctionInitial accelerationDesirable initial locations θ0, it is expected initial velocityAnd it is expected initial accelerationSo may be used
To obtain：
a0=θ (0) (2-1)
Further, after determining planning coefficient, formula (1) can be expressed as：
θ1(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5 (3)
Wherein, θ1(t) when representing time of running t, the expectation running position of motion control subobject.Formula (3) is carried out micro-
Divide and calculate, can obtain：
Wherein,When representing time of running t, the expectation speed of service of motion control subobject.Differential is carried out to formula (4)
It calculates, can obtain：
Wherein,When representing time of running t, the expectation operation acceleration of motion control subobject.
Further, the motion planning formula of formula (3), formula (4) and formula (5) for a motion control subobject.According to
The above method can construct the motion planning formula of total movement control subobject.It, can be by each initial ginseng during practical application
Manifold conjunction is represented and is calculated in the form of vectors, and is each worth a corresponding motion control subobject in vector.Using initial position as
, each it is worth the initial position for corresponding to a motion control subobject in corresponding vector.The advantage of doing so is that, it is only necessary to carry out
Motion planning can obtain the motion planning result of total movement control subobject.It should be noted that actually should
It, can be according to actual conditions at least one motion planning of construction selective in formula (3), formula (4) and formula (5) during being somebody's turn to do
Formula, and the time of running corresponding expectation operating parameter set is determined according to the motion planning formula of construction, it is expected operation ginseng
Manifold conjunction includes：It is expected speed of service set, it is expected running position set and it is expected to run at least one of acceleration set.
S230, the actual motion location sets and actual motion at obtain motion control object current time in the process of running
Sets of speeds.
Specifically, the actual motion position of current time each motion control subobject and actual motion speed are obtained respectively,
And form corresponding actual motion location sets and actual motion sets of speeds.
S240, the expectation running position set that current time is determined according to motion planning result, it is expected speed of service set
Acceleration set is run with expectation.
Illustratively, it is utilized respectively the expectation fortune that formula (3), formula (4) and formula (5) determine each motion control subobject
Line position puts, it is expected the speed of service and it is expected to run acceleration, and forms corresponding expectation running position set, it is expected the speed of service
Set and expectation run acceleration set.
S250, will it is expected running position set in respectively value and actual motion location sets in it is corresponding it is each value carry out subtraction meter
It calculates, to obtain site error set.
Specifically, when carrying out subtraction, it would be desirable to first value and actual motion location sets in the set of running position
In first value carry out subtraction, to determine the site error at motion control subobject current time that number is 1, successively class
It pushes away, determining whole site errors is formed into site error set.Each it is worth in site error set and represents corresponding movement
The difference value of position that control subobject it is expected to arrive with actual motion the position run at current time.
Wherein, the determining formula of site error set is：
E=θ1(t)-θ(t) (6)
Wherein, e represents site error set, and t represents the current time of running, θ1(t) represent that t moment it is expected running position collection
It closes, θ (t) represents the actual motion location sets of t moment.
S260, will it is expected speed of service set in respectively value and actual motion sets of speeds in it is corresponding it is each value carry out subtraction meter
It calculates, to obtain velocity error set.
Illustratively, the determining method class of the determining method of velocity error set and site error set seemingly, is not made again
It repeats.Wherein, each it is worth in velocity error set and represents that corresponding motion control subobject it is expected operation at current time
The difference value of speed and actual motion speed.
Wherein, the determining formula of velocity error set is：
Wherein,Representing velocity error set, t represents the current time of running,Represent that t moment it is expected speed of service collection
It closes,Represent t moment actual motion sets of speeds.
S270, default calculating is carried out to site error set and velocity error set, to obtain robust compensation parameter sets.
Illustratively, when determining motion control object control self-operating according to site error set and velocity error set
Meet the compensation control parameter of robustness, be denoted as robust compensation parameter sets.Wherein, each data in robust compensation parameter sets
Represent robust compensation parameter of the corresponding motion control subobject at current time.
Specifically, the step specifically includes：
Robust compensation parameter sets are obtained using following formula：
Wherein, v expressions robust compensation parameter sets, e expression site errors set,Represent velocity error set, ξ first
Set constant.In general, ξ can be set according to actual conditions, and more than 0.Further,Wherein, α is
Second setting constant, may be set according to actual conditions, and more than 0.| | η | | it is the norm of η,
β sets constant for third, and γ is the 4th setting constant.In the present embodiment, β is set as 3, γ 2.6, at this time, it is already possible to
Meets the needs of motion control object robustness.
S280, proportional plus integral control is carried out to site error set and velocity error set, to obtain closed loop feedback parameter
Set.
When carrying out double-closed-loop control to motion control object, advanced row position-force control, and according to position-force control
As a result velocity close-loop control is realized, to ensure the accuracy of closed loop feedback parameter finally obtained.
With reference to figure 2b, which specifically includes：
S281, proportional plus integral control is performed to being respectively worth in site error set, to obtain position controling parameter set.
Specifically, each motion control subobject actual motion position under current time can be determined according to site error set
With the difference value for it is expected running position, can adjust subsequent time according to the difference value makes each motion control subobject operation expire
Hope control parameter during running position, so that the running position of each motion control subobject of subsequent time is more accurate, Jin Ershi
Existing position-force control.Wherein, according to the difference value, to subsequent time, the control parameter of each motion control subobject is adjusted
When, it is determined as position controling parameter set using the control mode of proportional integration, and by adjustment result.
Wherein, proportional plus integral control formula is as follows：
up=kppe+kip∫edt (9)
Wherein, upRepresent position controling parameter set, e represents site error set, kppRepresent position-force control when ratio
Example coefficient, kipRepresent integral coefficient during position-force control.Wherein, kppAnd kipOccurrence can be carried out according to actual conditions
Setting.
S282, by respectively value each value corresponding with the controling parameter set of position does additional calculation in velocity error set, with
Obtain speed control parameter set.
In general, each motion control subobject current time actual motion speed can be determined according to speed difference set
Difference value with it is expected the speed of service, and can adjust subsequent time according to the difference value runs to each motion control subobject
The control parameter during speed of service it is expected, to realize velocity close-loop control.Wherein, in adjustment and the control parameter of velocity correlation
When, it not only needs to consider the speed difference value at current time, it is also necessary in view of the position difference value at current time.If it does not examine
Consider position difference value, even if being corrected to the speed of service, can not also ensure that motion control object runs to accurate position
On.Therefore, in the present embodiment, by respectively value each value corresponding with the controling parameter set of position makees addition meter in speed difference set
It calculates, with the influence that can be brought in subsequent control in view of speed difference value, it is also contemplated that being brought to position difference value
Influence, wherein, the specific formula of additional calculation is as follows：
Wherein, evRepresent speed control parameter set,Represent speed difference set, upRepresent position controling parameter set.
S283, proportional plus integral control is performed to being respectively worth in speed control parameter set, to obtain closed loop feedback parameter sets.
Specifically, using the control mode of proportional integration, after determining that control parameter relevant to the speed of service is adjusted
Obtained feedback control parameter set.
Wherein, proportional plus integral control formula is as follows：
upd=kpvev+kiv∫evdt (11)
Wherein, updRepresent feedback control parameter set, evRepresent speed control parameter set, kpvRepresent speed closed loop control
Proportionality coefficient when processed, kivRepresent integral coefficient during velocity close-loop control.Wherein, kpvAnd kivOccurrence can be according to practical feelings
Condition is set.Next time of running can be determined according to above-mentioned formula, closed-loop control is carried out to each motion control subobject
When feedback control parameter.
S290, it will respectively be worth in Torque Control parameter sets and expect to run corresponding each value progress addition in acceleration set
After calculating, Torque Control input vector is obtained.
Illustratively, since Torque Control parameter sets include robust compensation parameter sets and closed loop feedback parameter sets,
Therefore, when will respectively be worth in Torque Control parameter sets and it is expected to run corresponding each value progress additional calculation in acceleration set,
Can be by respectively respectively value and closed loop feedback parameter sets correspond in value, closed loop feedback parameter sets in robust compensation parameter sets
Each value carry out additional calculation.
Wherein, specific formula for calculation is as follows：
Wherein, u represents Torque Control input vector, and t represents the current time of running,It represents it is expected to run acceleration collection
It closes, updRepresent feedback control parameter set, v represents robust compensation parameter sets.
According to foregoing description it is found that not only including the robust of Guarantee control system robustness in Torque Control input vector
Compensating parameter set, the feedback control parameter set for realizing closed-loop control, further comprise the phase determined based on motion planning result
Accuracy when hoping operation acceleration set, and then ensure that according to Torque Control input vector control output torque.
S2100, after Torque Control input vector and inertia matrix are carried out multiplication calculating, with coriolis force matrix, gravitational moment
Battle array and frictional force matrix are added to obtain torque matrix, and using torque matrix as the driving moment of motion control object.
Illustratively, according to inverse dynamics of robot model it is found that the driving moment of motion control object and inertia matrix,
Coriolis force matrix, gravity matrix and frictional force matrix correlation.In the present embodiment, in order to ensure the accuracy of driving moment,
Torque Control input vector is added in inverse dynamics model.Specifically, according to Torque Control input vector control output torque
Specially according to Torque Control input vector control inertia matrix, be specially by Torque Control input vector and inertia matrix into
Row multiplication calculates.Further, multiplication result of calculation is added to obtain with coriolis force matrix, gravity matrix and frictional force matrix
Driving moment matrix.At this point, the formula of corresponding inverse dynamics model is as follows：
Wherein, τ represents driving moment matrix, and M (θ) represents inertia matrix, and u represents Torque Control input vector,
Represent coriolis force matrix, G (θ) represents gravity matrix, and f represents frictional force matrix.
Further, using each value in τ on diagonal successively as the output torque of motion control subobject.
S2110, using driving moment as the input of low-pass filter, and using the result obtained after filtering as motion control
The actual driving moment of object.
Specifically, in order to inhibit influence of the high-frequency noise to motion control object, in the present embodiment, low-pass filter is utilized
Eliminate the high-frequency noise in driving moment.
Specifically, it is described by taking low-pass first order filter as an example：
The Filtering Formula of low-pass first order filter is：
In formula (14), λ is cutoff frequency, and s is independent variable, and F (s) is Laplace transform amount.In practical applications, it is
Simplified computer realizes process, when low-pass first order filter filters, it is preferred to use difference-differential equation is specially：
Y (t)=aX (t)+(1-a) Y (t-T') (15)
Wherein, T' is the sample frequency of low-pass first order filter, can be identical with the sampling period of operating parameter, also may be used
With different from the sampling period of operating parameter, X (t) is the input signal at low-pass first order filter current time, i.e. driving moment
Matrix τ, t are current time, and Y (t-T') is corresponding defeated for previous sampling instant of the low-pass first order filter based on current time
Go out signal, a=λ 2 π T ', λ are the cutoff frequency of low-pass first order filter, and Y (t) is the output signal at current time, i.e., actually
Driving moment is denoted as τ '.
During according to the actual driving moment controlled motion control object obtained after filtering, it can inhibit high to a certain extent
Influence of the frequency noise to control result.
Technical solution provided in this embodiment, by carrying out motion planning, and according to motion planning to motion control object
As a result it determines the expectation speed of service set at current time, it is expected running position set and it is expected to run acceleration set, obtain
Actual motion location sets, the actual motion sets of speeds at current time, according to expectation running position set and actual motion position
It puts set and determines site error set, velocity error collection is determined according to expectation speed of service set and actual motion sets of speeds
It closes, and then robust compensation parameter sets and closed loop feedback parameter sets is determined according to site error set and velocity error set,
And according to robust compensation parameter sets, closed loop feedback parameter sets and it is expected that running acceleration set determines that Torque Control inputs
Vector, to determine to drive by Torque Control input vector, inertia matrix, coriolis force matrix, gravity matrix and frictional force matrix
Kinetic moment, and low-pass filtering is carried out to driving moment to obtain the technical solution of the actual driving moment of motion control object, it protects
The accuracy of the driving moment of motion control object has been demonstrate,proved, has realized the Dynamic High-accuracy tracking to motion control object, effectively
Inhibit external error and tracking error during tracking.
Exemplary description is carried out to method provided in this embodiment below, in this example, motion control object is installed
Robot structure diagram with reference to figure 1b.Specifically, host computer 11 performs the Torque Control method in this example, upper
Machine perform torque control method when, according to data flow be divided into motion planning module, torque compensation algoritic module and
EtherCAT master station modules, with reference to figure 2c, each intermodule is interacted by data-interface.Below in conjunction with Fig. 1 b and Fig. 2 c into
Row exemplary description：
Specifically, the creation method of motion planning module 21 can inherit the robot control software's programming increased income
The RTT of frame (Open Robot Control Software, OROCOS)::TaskContext classes, one OROCOS's of establishment
Real time capable module.Wherein, the interface 1 of motion planning module 21 can be that the operation calls method based on OROCOS is realized, be
Function call interface.When acting as receiving movement instruction of interface 1 triggers the control instruction receptance function of motion planning, and
Algorithm is called to calculate corresponding track.Wherein, movement instruction can be point-to-point movement, linear motion etc..It is moved with point-to-point
For, after interface 1 receives point-to-point movement instruction, quintic algebra curve interpolation algorithm can be called, so that motion planning mould
Block 21 generates the movement locus of a point-to-point.
Further, motion planning module 21 includes UpdateHook () member function.Motion planning
Module 21 at runtime, UpdateHook () member function real time execution, and with setpoint frequency, according to above-mentioned determining movement
The expectation speed of service in 13 each joint of mechanical arm body at trajectory calculation current time it is expected running position and it is expected operation
Acceleration, with specific reference to formula (3), formula (4) and formula (5), composition it is expected running position set θ1(t), it is expected operation
Sets of speedsAnd it is expected to run acceleration setWherein, the occurrence of setpoint frequency is not construed as limiting, such as
1000Hz。
It determines after it is expected operating parameter, utilizes the RTT of OROCOS::Input and RTT::Output methods realize interface 2
With interface 6.Wherein, interface 2 is specifically used for the expectation operation that setting interval sends each joint to torque compensation algoritic module 22
Speed it is expected running position and it is expected to run acceleration, that is, sets interval and send θ1(t)、AndInterface 6 has
Body is used to receive the actual motion position at the current time in each joint, the reality that the setting of torque backoff algorithm module 22 interval is sent
The border speed of service and actual motion acceleration, i.e. setting interval receive actual motion location sets θ (t), actual motion speed
SetAnd actual motion acceleration setWherein, interface 2 with interface 6 can identical also may be used by corresponding setting interval
With difference, occurrence may be set according to actual conditions, and such as be set as 1ms.
The creation method of torque compensation algoritic module 22 can be the RTT for inheriting OROCOS::TaskContext classes create
The Real time capable module of one OROCOS.
Specifically, torque compensation algoritic module 22 includes UpdateHook () member function.Torque compensation algorithm mould
When block 22 is run, UpdateHook () function performs Torque Control method provided in this embodiment with setpoint frequency, determines machinery
The Torque Control instruction of arm body 13.Wherein, Torque Control instruction includes the driving moment of mechanical arm body 13.Further,
Utilize the RTT of OROCOS::Input and RTT::Output methods realize interface 2 and interface 6, wherein, the tool of interface 2 and interface 6
Body is acted on referring to the description to interface 2 and interface 6 in motion planning module 22.Interface is realized using the transmission of shared drive
3 and interface 5.Wherein, interface 3 sends Torque Control instruction according to setting interval to EtherCAT master station modules.Interface 5 is according to setting
The θ (t) of fixed interval reception EtherCAT master station module transmissions,AndWherein, 5 corresponding setting of interface 3 and interface
Interval can be the same or different, and occurrence may be set according to actual conditions, and such as be set as 1ms.
Further, Fig. 2 d are the algorithm schematic block diagram of torque compensation method when UpdateHook () function calculates.With reference to
When Fig. 2 d, UpdateHook () function calculates, site error set e is determined using formula (6).Speed is determined using formula (7)
Error setAccording to e andRobust compensation parameter sets v is determined using formula (8).Reference formula (9) carries out proportional integration to e
Control, obtains position controling parameter set up, and according to formula (10) by upWithIt is added, using formula (11) to upWithIt is added
As a result proportional plus integral control is carried out, obtains feedback control parameter set upd.Further, using formula (12) by upd, v andAddition obtains Torque Control input vector u, and u is inputted in Inverse Dynamics of Manipulators model, is determined using formula (13)
Driving moment matrix τ, and τ is obtained into the actual driving moment τ ' of mechanical arm by low-pass filter.Wherein, τ ' is Torque Control
The particular content of instruction.
Specifically, EtherCAT master station modules 23 are realized using the real-time process of rt-preempt.Wherein, EtherCAT master
Module of standing 23 realizes interface 3 and interface 5 using the transmission of shared drive.Wherein, the specific effect of interface 3 and interface 5 is referring to power
Description in square backoff algorithm module to interface 3 and interface 5.Further, interface 4 is realized using EtherCAT communication protocols,
Torque Control is instructed and is sent by interface 4 to the corresponding each driver 12 of mechanical arm body 13, so that 12 basis of each driver
Torque Control instruction control machinery arm body 13 is moved.
Embodiment three
Fig. 3 is the structure diagram of a kind of moment control device that the embodiment of the present invention three provides.With reference to figure 3, this implementation
The moment control device that example provides specifically includes：Parameter acquisition module 301, parameter determination module 302 and Torque Control module
303。
Wherein, parameter acquisition module 301, for obtain motion control object in the process of running current time position miss
Difference set, velocity error set and expectation run acceleration set；Parameter determination module 302, for according to site error set
Torque Control parameter sets are determined with velocity error set, and Torque Control parameter sets include robust compensation parameter sets and closed loop
Feedback parameter set；Torque Control module 303, for running the control of acceleration set according to Torque Control parameter sets and expectation
The driving moment of motion control object.
Technical solution provided in this embodiment, the site error collection obtained in the process of running by motion control object
It closes, velocity error set determines Torque Control parameter sets, and according to being obtained in Torque Control parameter sets and operational process
It is expected the technical solution of the driving moment of acceleration set controlled motion control object so that the driving moment finally obtained is more
Accurately, the accuracy of dynamic tracking when ensure that double-closed-loop control, while avoid the external disturbance of control system.
On the basis of above-described embodiment, parameter determination module 302 includes：Robust parameter determination sub-module, for aligning
It puts error set and velocity error set carries out default calculating, to obtain robust compensation parameter sets；Feedback control submodule is used
In carrying out proportional plus integral control to site error set and velocity error set, to obtain closed loop feedback parameter sets.
On the basis of above-described embodiment, robust parameter determination sub-module is specifically used for：Utilize formulaRobust compensation parameter sets are obtained, wherein, v represents robust compensation parameter sets, and e represents site error collection
Close,Representing velocity error set, ξ is the first setting constant,α is the second setting constant, | | η | | it is the norm of η,β sets constant for third, and γ is the 4th setting constant.
On the basis of above-described embodiment, feedback control submodule includes：Position control unit, for site error collection
Respectively value performs proportional plus integral control in conjunction, to obtain position controling parameter set；Speed control unit, for by velocity error collection
Respectively value each value corresponding with the controling parameter set of position does additional calculation in conjunction, to obtain speed control parameter set；Feedback
Control unit, for performing proportional plus integral control to being respectively worth in speed control parameter set, to obtain closed loop feedback parameter sets.
On the basis of above-described embodiment, parameter acquisition module 301 includes：Actual parameter acquisition submodule, for obtaining
Motion control object the actual motion location sets at current time and actual motion sets of speeds in the process of running；It is expected parameter
Acquisition submodule, for determining the expectation running position set at current time according to motion planning result, it is expected speed of service collection
It closes and it is expected to run acceleration set；Site error determination sub-module, for that will it is expected in the set of running position respectively value and practical
Corresponding each value carries out subtraction in the set of running position, to obtain site error set；Velocity error determination sub-module is used
In that will it is expected that respectively corresponding each value carries out subtraction to value in actual motion sets of speeds in speed of service set, to obtain speed
Spend error set.
On the basis of above-described embodiment, further include：Initial parameter acquisition module is being transported for obtaining motion control object
Before the site error set at current time, velocity error set and expectation run acceleration set during row, movement is obtained
The initial parameter set at control object initial launch moment；Motion planning module, for being controlled according to initial parameter set to movement
Object processed carries out motion planning, to determine current time corresponding expectation operating parameter set according to motion planning result, it is expected
Operating parameter set includes it is expected speed of service set, it is expected running position set and it is expected to run acceleration set.
On the basis of above-described embodiment, Torque Control module 303 includes：Vectorial determination sub-module, for by torque control
After being respectively worth in parameter sets processed and it is expected that corresponding each value carries out additional calculation in operation acceleration set, it is defeated to obtain Torque Control
Incoming vector；Driving moment determination sub-module, after Torque Control input vector and inertia matrix are carried out multiplication calculating, with section
Family name's torque battle array, gravity matrix and frictional force matrix are added to obtain torque matrix, and using torque matrix as motion control object
Driving moment.
On the basis of above-described embodiment, further include：Low-pass filtering module, for according to Torque Control parameter sets and phase
It hopes after the driving moment of operation acceleration set controlled motion control object, using driving moment as the defeated of low-pass filter
Enter, and using the result obtained after filtering as the actual driving moment of motion control object.
Moment control device provided in an embodiment of the present invention is suitable for the Torque Control method that above-mentioned any embodiment provides,
Have corresponding function and advantageous effect.
Example IV
Fig. 4 is the structure diagram of a kind of robot that the embodiment of the present invention four provides, as shown in figure 4, the robot packet
Include processor 40, memory 41, input unit 42, output device 43 and motion control device 44；Processor 40 in robot
Quantity can be one or more, in Fig. 4 by taking a processor 40 as an example；It is processor 40, memory 41 in robot, defeated
Entering device 42, output device 43 and motion control device 44 can be connected by bus or other modes, with by total in Fig. 4
For line connection.Wherein, it is realized when processor 40 performs described program such as the Torque Control method in the embodiment of the present invention.Processing
Device 40 and memory 41 may be collectively referred to as host computer.Motion control device 44 be used for according to the torque that Torque Control method determines to
Amount is moved, including the driver that motion control object and drive motion control object move, wherein, driver and fortune
Dynamic control object electrical connection, motion control object includes at least two motion control subobjects, and each motion control is right
As being respectively provided with the motor that can be operated.
Memory 41 is used as a kind of computer readable storage medium, and journey is can perform available for storage software program, computer
Sequence and module, if the corresponding program instruction/module of Torque Control method in the embodiment of the present invention is (for example, Torque Control fills
Parameter acquisition module 301, parameter determination module 302 and Torque Control module 303 in putting.Processor 40 is stored by running
Software program, instruction and module in memory 41, so as to perform the various function application of robot and data processing,
Realize above-mentioned Torque Control method.
Memory 41 can mainly include storing program area and storage data field, wherein, storing program area can store operation system
Application program needed for system, at least one function；Storage data field can be stored uses created data etc. according to robot.
In addition, memory 41 can include high-speed random access memory, can also include nonvolatile memory, for example, at least one
Disk memory, flush memory device or other non-volatile solid state memory parts.In some instances, memory 41 can be into one
Step is included relative to the remotely located memory of processor 40, these remote memories can pass through network connection to robot.
The example of above-mentioned network includes but not limited to internet, intranet, LAN, mobile radio communication and combinations thereof.
Input unit 42 can be used for receiving input number or character information and generate with the user setting of robot with
And the key signals input that function control is related.Output device 43 may include that display screen etc. shows equipment.
Robot provided in this embodiment can be used for performing the Torque Control method that above-mentioned any embodiment provides, and have
Corresponding function and advantageous effect.
Embodiment five
The embodiment of the present invention five also provides a kind of storage medium for including computer executable instructions, and the computer can be held
When being performed by computer processor for performing a kind of Torque Control method, which includes for row instruction：
Obtain the site error set at current time, velocity error set and the expectation in the process of running of motion control object
Run acceleration set；
Torque Control parameter sets, Torque Control parameter sets packet are determined according to site error set and velocity error set
Include robust compensation parameter sets and closed loop feedback parameter sets；
The driving moment of acceleration set controlled motion control object is run according to Torque Control parameter sets and expectation.
Certainly, a kind of storage medium for including computer executable instructions that the embodiment of the present invention is provided, computer
The Torque Control method operation that executable instruction is not limited to the described above, can also be performed what any embodiment of the present invention was provided
Relevant operation in Torque Control method, and have corresponding function and advantageous effect.
By the description above with respect to embodiment, it is apparent to those skilled in the art that, the present invention
It can be realized by software and required common hardware, naturally it is also possible to which by hardware realization, but the former is more in many cases
Good embodiment.Based on such understanding, what technical scheme of the present invention substantially in other words contributed to the prior art
Part can be embodied in the form of software product, which can be stored in computer readable storage medium
In, floppy disk, read-only memory (Read-Only Memory, ROM), random access memory (Random such as computer
Access Memory, RAM), flash memory (FLASH), hard disk or CD etc., including some instructions with so that a computer is set
Standby (can be robot, personal computer, server or the network equipment etc.) performs the power described in each embodiment of the present invention
Square control method.
It is worth noting that, in the embodiment of above-mentioned moment control device, included each unit and module are only pressed
It is divided according to function logic, but is not limited to above-mentioned division, as long as corresponding function can be realized；In addition,
The specific name of each functional unit is also only to facilitate mutually distinguish, the protection domain being not intended to restrict the invention.
It should be appreciated that each section of the present invention can be realized with hardware, software, firmware or combination thereof.Above-mentioned
In embodiment, software that multiple steps or method can in memory and by suitable instruction execution system be performed with storage
Or firmware is realized.If for example, with hardware come realize in another embodiment, can be under well known in the art
Any one of row technology or their combination are realized：With for the logic gates to data-signal realization logic function
Discrete logic, have suitable combinational logic gate circuit application-specific integrated circuit, programmable gate array (PGA), scene
Programmable gate array (FPGA) etc..
In the description of this specification, reference term " one embodiment ", " example ", " is specifically shown " some embodiments "
The description of example " or " some examples " etc. means specific features, structure, material or the spy for combining the embodiment or example description
Point is contained at least one embodiment of the present invention or example.In the present specification, schematic expression of the above terms are not
Centainly refer to identical embodiment or example.Moreover, particular features, structures, materials, or characteristics described can be any
One or more embodiments or example in combine in an appropriate manner.
Note that it above are only presently preferred embodiments of the present invention and institute's application technology principle.It will be appreciated by those skilled in the art that
The present invention is not limited to specific embodiment described here, can carry out for a person skilled in the art various apparent variations,
It readjusts and substitutes without departing from protection scope of the present invention.Therefore, although being carried out by above example to the present invention
It is described in further detail, but the present invention is not limited only to above example, without departing from the inventive concept, also
It can include other more equivalent embodiments, and the scope of the present invention is determined by scope of the appended claims.
Claims (11)
- A kind of 1. Torque Control method, which is characterized in that including：Obtain motion control object the site error set at current time, velocity error set and expectation operation in the process of running Acceleration set；Torque Control parameter sets, the Torque Control ginseng are determined according to the site error set and the velocity error set Manifold conjunction includes robust compensation parameter sets and closed loop feedback parameter sets；According to the Torque Control parameter sets and the drive for it is expected to run the acceleration set control motion control object Kinetic moment.
- 2. Torque Control method according to claim 1, which is characterized in that described according to the site error set and institute It states velocity error set and determines that Torque Control parameter sets include：Default calculating is carried out to the site error set and the velocity error set, to obtain robust compensation parameter sets；Proportional plus integral control is carried out to the site error set and the velocity error set, to obtain closed loop feedback parameter set It closes.
- 3. Torque Control method according to claim 2, which is characterized in that described to the site error set and described Velocity error set carries out default calculating, is included with obtaining robust compensation parameter sets：Utilize formulaRobust compensation parameter sets are obtained, wherein, v represents robust compensation parameter sets, e tables Show site error set,Representing velocity error set, ξ is the first setting constant,α is the second setting constant, | | η | | it is the norm of η,β sets constant for third, and γ is the 4th setting constant.
- 4. Torque Control method according to claim 2, which is characterized in that described to the site error set and described Velocity error set carries out proportional plus integral control, is included with obtaining closed loop feedback parameter sets：To respectively value performs proportional plus integral control in the site error set, to obtain position controling parameter set；By respectively value each value corresponding with the position controling parameter set does additional calculation in the velocity error set, with To speed control parameter set；To respectively value performs proportional plus integral control in the speed control parameter set, to obtain closed loop feedback parameter sets.
- 5. Torque Control method according to claim 1, which is characterized in that the acquisition motion control object was being run The site error set at current time, velocity error set and expectation run acceleration set and include in journey：Obtain motion control object the actual motion location sets at current time and actual motion sets of speeds in the process of running；The expectation running position set at the current time is determined according to motion planning result, it is expected speed of service set and expectation Run acceleration set；By respectively corresponding each value carries out subtraction meter to value in the actual motion location sets in the set of the expectation running position It calculates, to obtain site error set；By respectively corresponding each value carries out subtraction meter to value in the actual motion sets of speeds in the expectation speed of service set It calculates, to obtain velocity error set.
- 6. Torque Control method according to claim 5, which is characterized in that the acquisition motion control object was being run Before the site error set at current time, velocity error set and expectation run acceleration set in journey, further include：Obtain the initial parameter set at motion control object initial launch moment；Motion planning is carried out to the motion control object according to the initial parameter set, to be determined according to motion planning result Current time is corresponding it is expected operating parameter set, the expectation operating parameter set include it is expected speed of service set, It is expected running position set and it is expected to run acceleration set.
- 7. Torque Control method according to claim 1, which is characterized in that described according to the Torque Control parameter sets It is expected that running acceleration set controls the driving moment of the motion control object to include with described：It will respectively be worth in the Torque Control parameter sets and it is expected to run corresponding each value in acceleration set and carry out additional calculation Afterwards, Torque Control input vector is obtained；After the Torque Control input vector and inertia matrix are carried out multiplication calculating, with coriolis force matrix, gravity matrix and Frictional force matrix is added to obtain torque matrix, and using the torque matrix as the driving moment of the motion control object.
- 8. Torque Control method according to claim 1, which is characterized in that described according to the Torque Control parameter sets It is expected after running the driving moment that acceleration set controls the motion control object with described, further include：Using the driving moment as the input of low-pass filter, and using the result obtained after filtering as motion control object Actual driving moment.
- 9. a kind of moment control device, which is characterized in that including：Parameter acquisition module, for site error set, the speed at obtain motion control object current time in the process of running Error set and expectation run acceleration set；Parameter determination module, for determining Torque Control parameter set according to the site error set and the velocity error set It closes, the Torque Control parameter sets include robust compensation parameter sets and closed loop feedback parameter sets；Torque Control module, described in running the control of acceleration set according to the Torque Control parameter sets and the expectation The driving moment of motion control object.
- 10. a kind of robot, which is characterized in that including：One or more processors；Memory, for storing one or more programs；When one or more of programs are performed by one or more of processors so that one or more of processors are real The now Torque Control method as described in claim 1-8 is any.
- 11. a kind of storage medium for including computer executable instructions, which is characterized in that the computer executable instructions by For execution such as Torque Control method according to any one of claims 1-8 when computer processor performs.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810045231.2A CN108189036B (en) | 2018-01-17 | 2018-01-17 | Torque control method and device, robot and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810045231.2A CN108189036B (en) | 2018-01-17 | 2018-01-17 | Torque control method and device, robot and storage medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN108189036A true CN108189036A (en) | 2018-06-22 |
| CN108189036B CN108189036B (en) | 2021-08-13 |
Family
ID=62590056
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810045231.2A Active CN108189036B (en) | 2018-01-17 | 2018-01-17 | Torque control method and device, robot and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108189036B (en) |
Cited By (14)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109138969A (en) * | 2018-07-17 | 2019-01-04 | 中国地质大学（武汉） | It is a kind of to creep into the prediction technique of state variable, equipment and storage equipment |
| CN109531565A (en) * | 2018-08-27 | 2019-03-29 | 苏州博众机器人有限公司 | A kind of control method of mechanical arm, device, service robot and storage medium |
| CN109807899A (en) * | 2019-03-08 | 2019-05-28 | 哈工大机器人（合肥）国际创新研究院 | For the cooperation robotic friction torque compensation method of dragging teaching |
| CN110471288A (en) * | 2019-08-28 | 2019-11-19 | 深圳市车安科技发展有限公司 | Gate self-adaptation control method, device and gate |
| CN110703687A (en) * | 2019-09-27 | 2020-01-17 | 上海畲贡自动化科技有限公司 | Error compensation system and method |
| CN111639749A (en) * | 2020-05-25 | 2020-09-08 | 上海智殷自动化科技有限公司 | Industrial robot friction force identification method based on deep learning |
| CN112116663A (en) * | 2020-08-20 | 2020-12-22 | 太仓中科信息技术研究院 | Offline programming method and system for camera robot and electronic equipment |
| CN112247992A (en) * | 2020-11-02 | 2021-01-22 | 中国科学院深圳先进技术研究院 | Robot feedforward torque compensation method |
| CN112405554A (en) * | 2020-11-06 | 2021-02-26 | 上海傅利叶智能科技有限公司 | Method and device for robot low-rigidity motion control and rehabilitation robot |
| CN113084828A (en) * | 2021-04-02 | 2021-07-09 | 上海电气集团股份有限公司 | Motion control method, device, equipment and storage medium |
| CN113894798A (en) * | 2021-11-26 | 2022-01-07 | 遨博（北京）智能科技有限公司 | Robot control method and robot system |
| CN113954078A (en) * | 2021-11-17 | 2022-01-21 | 深圳市优必选科技股份有限公司 | Force control joint control method and device, robot and readable storage medium |
| CN114147710A (en) * | 2021-11-27 | 2022-03-08 | 深圳市优必选科技股份有限公司 | Robot control method, device, robot and storage medium |
| CN116165883A (en) * | 2023-02-22 | 2023-05-26 | 北醒（北京）光子科技有限公司 | Servo control method, apparatus, electronic device, and computer-readable storage medium |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN1275470A (en) * | 1995-09-11 | 2000-12-06 | 株式会社安川电机 | Robot control equipment |
| CN103901776A (en) * | 2014-04-04 | 2014-07-02 | 哈尔滨工程大学 | Industrial mechanical arm anti-interference robustness self-adaptation PID control method |
| CN104647379A (en) * | 2015-01-19 | 2015-05-27 | 广东工业大学 | Dual-arm robot movement control method under non-linear condition of driver |
| CN106313044A (en) * | 2016-09-20 | 2017-01-11 | 华南理工大学 | Feedforward torque compensating method used for industrial robot |
| CN106950842A (en) * | 2017-05-18 | 2017-07-14 | 广州视源电子科技股份有限公司 | Decoupling control method and device, robot and storage medium |
| EP3171235A9 (en) * | 2015-11-19 | 2017-07-19 | Omron Corporation | Control device, control method, information processing program, and recording medium |
| CN107505835A (en) * | 2017-08-11 | 2017-12-22 | 广东工业大学 | A kind of method, apparatus and system of control machinery hands movement |
-
2018
- 2018-01-17 CN CN201810045231.2A patent/CN108189036B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN1275470A (en) * | 1995-09-11 | 2000-12-06 | 株式会社安川电机 | Robot control equipment |
| CN103901776A (en) * | 2014-04-04 | 2014-07-02 | 哈尔滨工程大学 | Industrial mechanical arm anti-interference robustness self-adaptation PID control method |
| CN104647379A (en) * | 2015-01-19 | 2015-05-27 | 广东工业大学 | Dual-arm robot movement control method under non-linear condition of driver |
| EP3171235A9 (en) * | 2015-11-19 | 2017-07-19 | Omron Corporation | Control device, control method, information processing program, and recording medium |
| CN106313044A (en) * | 2016-09-20 | 2017-01-11 | 华南理工大学 | Feedforward torque compensating method used for industrial robot |
| CN106950842A (en) * | 2017-05-18 | 2017-07-14 | 广州视源电子科技股份有限公司 | Decoupling control method and device, robot and storage medium |
| CN107505835A (en) * | 2017-08-11 | 2017-12-22 | 广东工业大学 | A kind of method, apparatus and system of control machinery hands movement |
Cited By (20)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109138969A (en) * | 2018-07-17 | 2019-01-04 | 中国地质大学（武汉） | It is a kind of to creep into the prediction technique of state variable, equipment and storage equipment |
| CN109531565A (en) * | 2018-08-27 | 2019-03-29 | 苏州博众机器人有限公司 | A kind of control method of mechanical arm, device, service robot and storage medium |
| CN109531565B (en) * | 2018-08-27 | 2023-11-03 | 苏州博众智能机器人有限公司 | Control method and device of mechanical arm, service robot and storage medium |
| CN109807899A (en) * | 2019-03-08 | 2019-05-28 | 哈工大机器人（合肥）国际创新研究院 | For the cooperation robotic friction torque compensation method of dragging teaching |
| CN109807899B (en) * | 2019-03-08 | 2020-09-29 | 哈工大机器人（合肥）国际创新研究院 | Friction torque compensation method of cooperative robot for dragging teaching |
| CN110471288B (en) * | 2019-08-28 | 2022-10-11 | 深圳市车安科技发展有限公司 | Self-adaptive control method and device for gate and gate |
| CN110471288A (en) * | 2019-08-28 | 2019-11-19 | 深圳市车安科技发展有限公司 | Gate self-adaptation control method, device and gate |
| CN110703687A (en) * | 2019-09-27 | 2020-01-17 | 上海畲贡自动化科技有限公司 | Error compensation system and method |
| CN111639749A (en) * | 2020-05-25 | 2020-09-08 | 上海智殷自动化科技有限公司 | Industrial robot friction force identification method based on deep learning |
| CN112116663A (en) * | 2020-08-20 | 2020-12-22 | 太仓中科信息技术研究院 | Offline programming method and system for camera robot and electronic equipment |
| CN112247992A (en) * | 2020-11-02 | 2021-01-22 | 中国科学院深圳先进技术研究院 | Robot feedforward torque compensation method |
| CN112405554A (en) * | 2020-11-06 | 2021-02-26 | 上海傅利叶智能科技有限公司 | Method and device for robot low-rigidity motion control and rehabilitation robot |
| CN112405554B (en) * | 2020-11-06 | 2022-07-01 | 上海傅利叶智能科技有限公司 | Method and device for robot low-rigidity motion control and rehabilitation robot |
| CN113084828A (en) * | 2021-04-02 | 2021-07-09 | 上海电气集团股份有限公司 | Motion control method, device, equipment and storage medium |
| CN113954078A (en) * | 2021-11-17 | 2022-01-21 | 深圳市优必选科技股份有限公司 | Force control joint control method and device, robot and readable storage medium |
| CN113954078B (en) * | 2021-11-17 | 2023-02-24 | 深圳市优必选科技股份有限公司 | Force control joint control method and device, robot and readable storage medium |
| CN113894798A (en) * | 2021-11-26 | 2022-01-07 | 遨博（北京）智能科技有限公司 | Robot control method and robot system |
| CN114147710A (en) * | 2021-11-27 | 2022-03-08 | 深圳市优必选科技股份有限公司 | Robot control method, device, robot and storage medium |
| CN114147710B (en) * | 2021-11-27 | 2023-08-11 | 深圳市优必选科技股份有限公司 | Robot control method and device, robot and storage medium |
| CN116165883A (en) * | 2023-02-22 | 2023-05-26 | 北醒（北京）光子科技有限公司 | Servo control method, apparatus, electronic device, and computer-readable storage medium |
Also Published As
| Publication number | Publication date |
|---|---|
| CN108189036B (en) | 2021-08-13 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108189036A (en) | Torque control method and device, robot and storage medium | |
| Xu et al. | Output feedback adaptive robust precision motion control of linear motors | |
| US10481566B2 (en) | Machine learning device, servo control device, servo control system and machine learning method | |
| CN109927032A (en) | A kind of mechanical arm Trajectory Tracking Control method based on High-Order Sliding Mode observer | |
| Vick et al. | Model predictive control as a service—Concept and architecture for use in cloud-based robot control | |
| JP2017102617A (en) | Correction device, control method of correction device, information processing program, and record medium | |
| CN107703750A (en) | Networked multi-axis motion position synchronous control method based on active disturbance rejection controller | |
| JP6091523B2 (en) | Servo control device | |
| CN114450131A (en) | Non-derivative model learning system and design for robot system | |
| CN112077839B (en) | Motion control method and device for mechanical arm | |
| JP2014136260A (en) | Control device | |
| Yook et al. | A design methodology for distributed control systems to optimize performance in the presence of time delays | |
| CN104423303A (en) | Sizing and tuning methodology for optimized motion control components and energy efficiency | |
| Olsson | Modeling and control of a Delta-3 robot | |
| CN115509135A (en) | Laser scanning galvanometer model prediction control method and system | |
| CN106950842B (en) | Decoupling control method and device, robot and storage medium | |
| KR101983946B1 (en) | Machining of workpieces with model-supported error compensation | |
| CN112847373B (en) | Robot track synchronous control method and computer readable storage medium | |
| CN111015661B (en) | Active vibration control method and system for flexible load of robot | |
| WO2006046500A1 (en) | Remote control system for transmitting and receiving signals via communication path having communication delay | |
| CN111152213B (en) | Mechanical arm vibration compensation method and device based on hybrid control | |
| CN107511830A (en) | A kind of series parallel robot in five degrees of freedom controller parameter adaptively adjusts implementation method | |
| JPS58169212A (en) | Position controller of servomotor | |
| WO2020162199A1 (en) | Control device, model creation method, control program | |
| Cuong et al. | Learning feed-forward control for a two-link rigid robot arm |
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 |