CN109597437B - Haptic interaction control method and device based on speed sensor and differentiator force - Google Patents

Haptic interaction control method and device based on speed sensor and differentiator force Download PDF

Info

Publication number
CN109597437B
CN109597437B CN201811485955.5A CN201811485955A CN109597437B CN 109597437 B CN109597437 B CN 109597437B CN 201811485955 A CN201811485955 A CN 201811485955A CN 109597437 B CN109597437 B CN 109597437B
Authority
CN
China
Prior art keywords
actuator
time
differentiator
sensor
angular velocity
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201811485955.5A
Other languages
Chinese (zh)
Other versions
CN109597437A (en
Inventor
田大鹏
王中石
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Changchun Institute of Optics Fine Mechanics and Physics of CAS
Original Assignee
Changchun Institute of Optics Fine Mechanics and Physics of CAS
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Changchun Institute of Optics Fine Mechanics and Physics of CAS filed Critical Changchun Institute of Optics Fine Mechanics and Physics of CAS
Priority to CN201811485955.5A priority Critical patent/CN109597437B/en
Publication of CN109597437A publication Critical patent/CN109597437A/en
Application granted granted Critical
Publication of CN109597437B publication Critical patent/CN109597437B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D13/00—Control of linear speed; Control of angular speed; Control of acceleration or deceleration, e.g. of a prime mover
    • G05D13/62—Control of linear speed; Control of angular speed; Control of acceleration or deceleration, e.g. of a prime mover characterised by the use of electric means, e.g. use of a tachometric dynamo, use of a transducer converting an electric value into a displacement

Landscapes

  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Engineering & Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Feedback Control In General (AREA)
  • Control Of Position Or Direction (AREA)
  • Control Of Electric Motors In General (AREA)

Abstract

According to the force touch interaction method and device, the angular velocity and the angular acceleration of the actuator are acquired through the velocity sensor, the differentiator is used for processing the data to obtain the control quantity of the actuator, the actuator is accurately controlled, and force touch interaction operation is achieved.

Description

Haptic interaction control method and device based on speed sensor and differentiator force
Technical Field
The invention relates to the field of automatic control, in particular to a force touch interactive control method and device based on a speed sensor and a differentiator.
Background
Human invented the telephone in 19 th century, realized the transmission of the sense of hearing; in the 20 th century, the advent of cameras and televisions enabled visual transmission; in the 21 st century, tactile transmission was also possible using mechanical devices, actuators and control methods. In the process of understanding nature and reforming nature, people usually only do work on the object around, and apply acting force on the object in the operation process and feel the reacting force from the object at any time. The force-tactile interaction system is capable of separating a person in the process from the manipulated object, i.e. the operator and the manipulated object may be separate. The system transmits the movement of the operator to the operated object, and realizes the relation between the acting force and the reacting force between the operator and the operated object, and a physical medium is formed between the operator and the operated object. Force sensors are often required to measure the forces between the actuator and the operator and between the actuator and the object being manipulated. However, the force sensor not only increases the system cost, but also makes it difficult to realize a small, lightweight force-tactile interaction device due to limitations in measurement accuracy, bandwidth, mounting size, weight, and the like.
Disclosure of Invention
In view of this, embodiments of the present invention provide a method and an apparatus for controlling haptic interaction based on a velocity sensor and a differentiator, which can facilitate haptic interaction.
In a first aspect, the present invention provides a haptic interaction control method based on a speed sensor and a force of a differentiator, wherein a dead loop is defined in a main function in advance, the dead loop uses a variable as a flag bit to judge whether to exit the loop, a timer is used to interrupt the loop at fixed time intervals, the kth time entering a timer interrupt service function is defined as a time k, and the time entering the timer interrupt service function before the time k is called as a time k-1, and the method comprises:
s101, processing by using the angular velocity of the actuator at the moment k of the discrete differentiator with feedforward to obtain the angular acceleration of the actuator:
Figure BDA0001894483830000021
Figure BDA0001894483830000022
Figure BDA0001894483830000023
Figure BDA0001894483830000024
wherein, TsIs an interrupt period of the timer and is,
Figure BDA0001894483830000025
is the sensor data read by the speed sensor of actuator i at time k,
Figure BDA0001894483830000026
is the low pass filtered value of the velocity sensor reading corresponding to the actuator i at time k,
Figure BDA0001894483830000027
and
Figure BDA0001894483830000028
an intermediate variable for differential calculation of the angular velocity of the actuator i,
Figure BDA0001894483830000029
is the micro of the velocity sensor reading of actuator i at time kThe output of the divider is used for dividing the signal into a plurality of signals,
Figure BDA00018944838300000210
and
Figure BDA00018944838300000211
the gains of the angular velocity differentiator of the actuator i are constants larger than 0;
s102, control quantity u of actuator driver output to actuator i at k-1 momenti(k-1) filtering;
Figure BDA00018944838300000212
where g is the gain of the filter,
Figure BDA00018944838300000213
is calculated at time ki(k-1) low pass filtered values;
s103, calculating to obtain an intermediate variable taui(k) Of intermediate variable τi(k) Including the information that the actuator i receives external acting force at the moment k, and the intermediate variable taui(k) Comprises the following steps:
Figure BDA00018944838300000214
wherein J is max { J ═ J1,J2,...,JnIs the largest one of the equivalent moments of inertia of all actuators; b ═ max { B1,B2,...,BnIs the largest of the equivalent damping of all actuators, JiAnd Bi(i ═ 1,2, …, n) denotes the equivalent moment of inertia and equivalent damping of the actuator i, respectively;
s104, calculating the integral value of the angular velocity of the actuator
Figure BDA00018944838300000215
The integral value
Figure BDA00018944838300000216
Comprises the following steps:
Figure BDA00018944838300000219
wherein the content of the first and second substances,
Figure BDA00018944838300000217
for the actuator angular velocity at time k read by the sensor,
Figure BDA00018944838300000220
the integral value of the angular speed of the actuator at the moment k;
s105, calculating the control quantity output to the actuator driver of the actuator i at the moment k by utilizing the following relation;
Figure BDA0001894483830000031
wherein, K1And K2The gains for position and velocity, respectively, are constants greater than 0.
Alternatively, the calculating of the integral value of the angular velocity of the actuator
Figure BDA0001894483830000032
The method comprises the following steps:
calculating integral value of angular velocity of actuator by using Euler method
Figure BDA0001894483830000033
Optionally, the method further comprises:
judging whether to finish the control, if so, jumping out of the dead cycle in a mode of setting a flag bit, and finishing the control; if not, the flag bit is not set, and the next timer interruption is waited.
Optionally, judging whether to finish control, if so, jumping out of a dead cycle in a mode of setting a flag bit, and finishing control; if not, not setting the flag bit, and waiting for the next timer interrupt, further comprising:
and receiving sensor data sent by other control circuits or sending sensor data to other control circuits.
Optionally, before the processing the angular velocity of the actuator at the time k by using the discrete differentiator with feedforward to obtain the angular acceleration of the actuator, the method further includes:
and judging the type of the sensor, and executing the subsequent steps if the type of the sensor is a speed sensor.
Optionally, the filter is a first order low pass filter, a second order higher order low pass filter, a butterworth filter, or an IIR digital filter.
Optionally, the speed sensor employs a tachogenerator.
In a second aspect, the present invention provides a haptic interaction control device based on a speed sensor and a differentiator force, the device comprising:
the presetting unit is used for defining an endless loop in the main function in advance, the endless loop judges whether to exit the loop by using a variable as a flag bit, the loop is interrupted once every fixed time by using a timer, the kth time entering of the timer interrupt service function is defined as the time k, and the time entering of the timer interrupt service function before the time k is called as the time k-1;
the processing unit is used for processing the angular velocity of the actuator at the moment k by using the discrete differentiator with feedforward to obtain the angular acceleration of the actuator:
Figure BDA0001894483830000034
Figure BDA0001894483830000041
Figure BDA0001894483830000042
Figure BDA0001894483830000043
wherein, TsTo timeThe period of interruption of the device is as follows,
Figure BDA0001894483830000044
is the sensor data read by the speed sensor of actuator i at time k,
Figure BDA0001894483830000045
is the low pass filtered value of the velocity sensor reading corresponding to the actuator i at time k,
Figure BDA0001894483830000046
and
Figure BDA0001894483830000047
an intermediate variable for differential calculation of the angular velocity of the actuator i,
Figure BDA0001894483830000048
is the differentiator output for the velocity sensor reading at time k for actuator i,
Figure BDA0001894483830000049
and
Figure BDA00018944838300000410
the gains of the angular velocity differentiator of the actuator i are constants larger than 0;
a filter unit for outputting the control quantity u of the actuator driver to the actuator i at the time k-1i(k-1) filtering;
Figure BDA00018944838300000411
where g is the gain of the filter,
Figure BDA00018944838300000412
is calculated at time ki(k-1) low pass filtered values;
a first calculating unit for calculating an intermediate variable τi(k) Of intermediate variable τi(k) Including actuator i being subjected to at time kInformation of the applied force to the outside, the intermediate variable taui(k) Comprises the following steps:
Figure BDA00018944838300000413
wherein J is max { J ═ J1,J2,...,JnIs the largest one of the equivalent moments of inertia of all actuators; b ═ max { B1,B2,...,BnIs the largest of the equivalent damping of all actuators, JiAnd Bi(i ═ 1,2, …, n) denotes the equivalent moment of inertia and equivalent damping of the actuator i, respectively;
a second calculation unit for calculating an integral value of the angular velocity of the actuator
Figure BDA00018944838300000414
The integral value
Figure BDA00018944838300000415
Comprises the following steps:
Figure BDA00018944838300000416
wherein the content of the first and second substances,
Figure BDA00018944838300000417
for the actuator angular velocity at time k read by the sensor,
Figure BDA00018944838300000418
the integral value of the angular speed of the actuator at the moment k;
the output unit is used for calculating and obtaining the control quantity output to the actuator driver of the actuator i at the moment k by utilizing the following relation;
Figure BDA00018944838300000419
wherein, K1And K2The gains for position and velocity, respectively, are constants greater than 0.
Optionally, the first computing unit is specifically configured to:
calculating integral value of angular velocity of actuator by using Euler method
Figure BDA0001894483830000051
Optionally, the method further comprises:
the judging unit is used for judging whether to finish the control, if so, jumping out of the dead cycle in a mode of setting a flag bit, and finishing the control; if not, the flag bit is not set, and the next timer interruption is waited.
According to the force touch interaction method and device, the angular velocity and the angular acceleration of the actuator are acquired through the velocity sensor, the differentiator is used for processing the data to obtain the control quantity of the actuator, the actuator is accurately controlled, and force touch interaction operation is achieved.
Drawings
FIG. 1 is a flow chart of one embodiment of a haptic interaction control method based on a velocity sensor and a differentiator force in an embodiment of the present invention;
FIG. 2 is a flow chart of another embodiment of a haptic interaction control method based on a velocity sensor and a differentiator force in an embodiment of the present invention;
FIG. 3 is a block diagram of one embodiment of a haptic interaction control device based on a velocity sensor and a differentiator force in an embodiment of the invention.
Detailed Description
In order to make the technical solutions of the present invention better understood, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
The terms "first," "second," "third," "fourth," and the like in the description and in the claims, as well as in the drawings, are used for distinguishing between similar elements and not necessarily for describing a particular sequential or chronological order. It will be appreciated that the data so used may be interchanged under appropriate circumstances such that the embodiments described herein may be practiced otherwise than as specifically illustrated or described herein. Furthermore, the terms "comprises," "comprising," and "having," and any variations thereof, are intended to cover a non-exclusive inclusion, such that a process, method, system, article, or apparatus that comprises a list of steps or elements is not necessarily limited to those steps or elements expressly listed, but may include other steps or elements not expressly listed or inherent to such process, method, article, or apparatus.
Referring to fig. 1, the present invention provides a haptic interaction control method based on a speed sensor and a differentiator force, wherein a dead loop is defined in a main function in advance, the dead loop uses a variable as a flag bit to determine whether to exit the loop, a timer is used to interrupt the loop at fixed time intervals, a kth time entering timer interrupt service function is defined as a time k, and a time entering the timer interrupt service function before the time k is referred to as a time k-1, and the method includes:
s101, processing by using the angular velocity of the actuator at the moment k of the discrete differentiator with feedforward to obtain the angular acceleration of the actuator:
Figure BDA0001894483830000061
Figure BDA0001894483830000062
Figure BDA0001894483830000063
Figure BDA0001894483830000064
wherein, TsIs an interrupt period of the timer and is,
Figure BDA0001894483830000065
is the sensor data read by the speed sensor of actuator i at time k,
Figure BDA0001894483830000066
is the low pass filtered value of the velocity sensor reading corresponding to the actuator i at time k,
Figure BDA0001894483830000067
and
Figure BDA0001894483830000068
an intermediate variable for differential calculation of the angular velocity of the actuator i,
Figure BDA0001894483830000069
is the differentiator output for the velocity sensor reading at time k for actuator i,
Figure BDA00018944838300000610
and
Figure BDA00018944838300000611
the gains of the angular velocity differentiator of the actuator i are constants larger than 0;
s102, control quantity u of actuator driver output to actuator i at k-1 momenti(k-1) filtering;
Figure BDA00018944838300000612
where g is the gain of the filter,
Figure BDA00018944838300000613
is calculated at time ki(k-1) low pass filtered values.
S103, calculating to obtain an intermediate variable taui(k) Of intermediate variable τi(k) Including the information that the actuator i receives external acting force at the moment k, and the intermediate variable taui(k) Comprises the following steps:
Figure BDA00018944838300000614
wherein J is max { J ═ J1,J2,...,JnIs the largest one of the equivalent moments of inertia of all actuators; b ═ max { B1,B2,...,BnIs the largest of the equivalent damping of all actuators, JiAnd Bi(i ═ 1,2, …, n) denotes the equivalent moment of inertia and equivalent damping of the actuator i, respectively.
S104, calculating the integral value of the angular velocity of the actuator
Figure BDA0001894483830000071
The integral value
Figure BDA0001894483830000072
Comprises the following steps:
Figure BDA0001894483830000073
wherein the content of the first and second substances,
Figure BDA0001894483830000074
for the actuator angular velocity at time k read by the sensor,
Figure BDA0001894483830000075
the integral value of the angular speed of the actuator at the moment k;
s105, calculating the control quantity output to the actuator driver of the actuator i at the moment k by utilizing the following relation;
Figure BDA0001894483830000076
wherein, K1And K2The gains for position and velocity, respectively, are constants greater than 0.
In this embodiment, the speed sensor employs a tachogenerator, and the actuator may employ an actuator, an electro-hydraulic servo mechanism, a hydraulic drive mechanism, and an ultrasonic motor, which are not limited to this.
According to the force touch interaction method provided by the invention, the angular velocity and the angular acceleration of the actuator are subjected to data acquisition through the velocity sensor, the data are processed by the differentiator to obtain the control quantity of the actuator, the actuator is accurately controlled, and the force touch interaction operation is realized.
In step S104, an integrated value of the angular velocity of the actuator is calculated by the euler method
Figure BDA0001894483830000077
Other methods of calculating the integral may be used, and are not limited in this regard.
As shown in fig. 2, after step S105, the method further includes:
s106, judging whether to finish control;
s107, if yes, jumping out of the dead cycle in a mode of setting a flag bit, and ending the control;
and S108, if not, not setting the zone bit and waiting for the next timer interruption.
In the dead loop, the program will execute the operations of reading speed sensor data and outputting control quantity to actuator driver, and when the distributed control circuit scheme is adopted, the program will send the local speed sensor data and the intermediate variable tau containing force information obtained according to the above process to other n-1 control circuits through communication interface besides the above operationsi(k)。
When a distributed control scheme is adopted, one circuit board controls one actuator, whether control is finished or not is judged, if yes, a flag bit is set to jump out of a dead cycle, and control is finished; if not, not setting the flag bit, and waiting for the next timer interrupt, further comprising:
and receiving sensor data sent by other control circuits or sending sensor data to other control circuits.
When one circuit board controls one actuator, only one sensor data interface and one actuator driver are needed on the circuit board. The communication interface realizes data communication among the n circuit boards. The specific design forms of RS-485 bus, CAN bus, Ethernet and the like CAN be adopted. When the actuator is designed to have multiple degrees of freedom, one actuator may also be driven by multiple driving actuators, and the changes needed are only to increase the number of sensor data interfaces and actuator drivers on the circuit board, and the most important control algorithms are the same.
Further, before step S101, the method further includes:
s100, judging the type of the sensor, and executing the subsequent steps if the type of the sensor is a speed sensor, wherein the control method needs to utilize a differentiator to obtain the angular speed and the angular acceleration of the actuator, and when the speed sensor is adopted, the differentiator is only needed to estimate the angular acceleration once; when the position sensor is used, the angular velocity and the angular acceleration need to be estimated using a differentiator twice, and the processing procedure may be different depending on the type of sensor.
Alternatively, the filter may be a first-order low-pass filter, a second-order high-pass filter, a high-order low-pass filter, a butterworth filter, or an IIR digital filter, and it should be noted that the form of the filter may be various and may be flexibly selected by one of ordinary skill in the art.
In equation (5) of S102, the control algorithm actually includes three parts: position control, speed control, force control. Each part is controlled by a proportional controller, and the equivalent control gain of the force control part is 1. In the actual design, according to the difference of the system characteristics, a more complex controller form can be adopted to replace the proportional controller, and the difference is only that the proportional calculation is changed into other algorithms, which is not described herein again.
It should be noted that, the force touch interaction among a plurality of actuators can be realized by using the control algorithm, an operator can arbitrarily select one actuator as a host to operate, and the action can be transmitted to the rest n-1 actuators; the force of the interaction between the rest n-1 actuating mechanisms and other operators or operated objects can be transmitted under the action of the device and the algorithm.
Correspondingly, the invention also provides a haptic interaction control device based on a speed sensor and a differentiator force, which is shown in the figure 3 and comprises:
a presetting unit 301, configured to define an endless loop in a main function in advance, where the endless loop uses a variable as a flag bit to determine whether to exit the loop, and uses a timer to interrupt the loop every fixed time, and defines a kth time entering timer interrupt service function as a time k, and a time entering the timer interrupt service function before the time k is referred to as a time k-1;
a processing unit 302, configured to process the angular velocity of the actuator at time k by using a discrete differentiator with feedforward to obtain an angular acceleration of the actuator:
Figure BDA0001894483830000091
Figure BDA0001894483830000092
Figure BDA0001894483830000093
Figure BDA0001894483830000094
wherein, TsIs an interrupt period of the timer and is,
Figure BDA0001894483830000095
is the sensor data read by the speed sensor of actuator i at time k,
Figure BDA0001894483830000096
is the low pass filtered value of the velocity sensor reading corresponding to the actuator i at time k,
Figure BDA0001894483830000097
and
Figure BDA0001894483830000098
an intermediate variable for differential calculation of the angular velocity of the actuator i,
Figure BDA0001894483830000099
is the differentiator output for the velocity sensor reading at time k for actuator i,
Figure BDA00018944838300000910
and
Figure BDA00018944838300000911
the gains of the angular velocity differentiator of the actuator i are constants larger than 0;
a filter unit 303 for outputting the control quantity u of the actuator driver to the actuator i at the time k-1i(k-1) filtering;
Figure BDA00018944838300000912
where g is the gain of the filter,
Figure BDA00018944838300000913
is calculated at time ki(k-1) low pass filtered values;
a first calculating unit 304 for calculating an intermediate variable τi(k) Of intermediate variable τi(k) Including the information that the actuator i receives external acting force at the moment k, and the intermediate variable taui(k) Comprises the following steps:
Figure BDA00018944838300000914
wherein J is max { J ═ J1,J2,...,JnIs the largest one of the equivalent moments of inertia of all actuators; b ═ max { B1,B2,...,BnIs the largest of the equivalent damping of all actuators, JiAnd Bi(i ═ 1,2, …, n) denotes the equivalent moment of inertia and equivalent damping of the actuator i, respectively;
a second calculation unit 305 for calculating an integral value of the angular velocity of the actuator
Figure BDA00018944838300000915
The integral value
Figure BDA00018944838300000916
Comprises the following steps:
Figure BDA00018944838300000917
wherein the content of the first and second substances,
Figure BDA00018944838300000920
for the actuator angular velocity at time k read by the sensor,
Figure BDA00018944838300000919
the integral value of the angular speed of the actuator at the moment k;
the output unit 306 is used for calculating and obtaining the control quantity output to the actuator driver of the actuator i at the moment k by utilizing the following relation;
Figure BDA0001894483830000101
wherein, K1And K2The gains for position and velocity, respectively, are constants greater than 0.
Optionally, the first calculating unit 304 is specifically configured to:
calculating integral value of angular velocity of actuator by using Euler method
Figure BDA0001894483830000102
Optionally, the method further comprises:
a judging unit 307, configured to judge whether to end the control, and if so, jump out of the dead cycle by setting a flag bit, and end the control; if not, the flag bit is not set, and the next timer interruption is waited.
According to the force touch interaction device, the angular velocity and the angular acceleration of the actuator are subjected to data acquisition through the velocity sensor, the data are processed through the differentiator to obtain the control quantity of the actuator, the actuator is accurately controlled, and force touch interaction operation is achieved.
It is clear to those skilled in the art that, for convenience and brevity of description, the specific working processes of the above-described systems, apparatuses and units may refer to the corresponding processes in the foregoing method embodiments, and are not described herein again.
In the several embodiments provided in the present application, it should be understood that the disclosed system, apparatus and method may be implemented in other manners. For example, the above-described apparatus embodiments are merely illustrative, and for example, the division of the units is only one logical division, and other divisions may be realized in practice, for example, a plurality of units or components may be combined or integrated into another system, or some features may be omitted, or not executed. In addition, the shown or discussed mutual coupling or direct coupling or communication connection may be an indirect coupling or communication connection through some interfaces, devices or units, and may be in an electrical, mechanical or other form.
The units described as separate parts may or may not be physically separate, and parts displayed as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of the embodiment.
In addition, functional units in the embodiments of the present invention may be integrated into one processing unit, or each unit may exist alone physically, or two or more units are integrated into one unit. The integrated unit can be realized in a form of hardware, and can also be realized in a form of a software functional unit.
Those skilled in the art will appreciate that all or part of the steps in the methods of the above embodiments may be implemented by associated hardware instructed by a program, which may be stored in a computer-readable storage medium, and the storage medium may include: a Read Only Memory (ROM), a Random Access Memory (RAM), a magnetic or optical disk, or the like.
While the haptic interaction control method and device based on the velocity sensor and the differentiator provided by the present invention have been described in detail, those skilled in the art will appreciate that the present disclosure is not limited thereto.

Claims (10)

1. A haptic interaction control method based on a speed sensor and a differentiator force is characterized in that a dead loop is defined in a main function in advance, the dead loop judges whether to exit the loop by using a variable as a flag bit, the loop is interrupted once every fixed time by using a timer, the k-th time entering timer interrupt service function is defined as the time k, and the time entering the timer interrupt service function before the time k is called as the time k-1, and the method comprises the following steps:
s101, processing the angular velocity of the actuator at the moment k of the discrete differentiator with feedforward to obtain the angular acceleration of the actuator:
Figure FDA0002439996000000011
Figure FDA0002439996000000012
Figure FDA0002439996000000013
Figure FDA0002439996000000014
wherein, TsIs an interrupt period of the timer and is,
Figure FDA0002439996000000015
is the sensor data read by the speed sensor of actuator i at time k,
Figure FDA0002439996000000016
is the low pass filtered value of the velocity sensor reading corresponding to the actuator i at time k,
Figure FDA0002439996000000017
and
Figure FDA0002439996000000018
an intermediate variable for differential calculation of the angular velocity of the actuator i,
Figure FDA0002439996000000019
is the differentiator output for the velocity sensor reading at time k for actuator i,
Figure FDA00024399960000000110
and
Figure FDA00024399960000000111
the gains of the angular velocity differentiator of the actuator i are constants larger than 0;
s102, control quantity u of actuator driver output to actuator i at k-1 momenti(k-1) filtering;
Figure FDA00024399960000000112
where g is the gain of the filter,
Figure FDA00024399960000000113
is calculated at time ki(k-1) low pass filtered values;
s103, countingCalculating an intermediate variable τi(k) Of intermediate variable τi(k) Including the information that the actuator i receives external acting force at the moment k, and the intermediate variable taui(k) Comprises the following steps:
Figure FDA00024399960000000114
wherein J is max { J ═ J1,J2,...,JnIs the largest one of the equivalent moments of inertia of all actuators; b ═ max { B1,B2,...,BnIs the largest of the equivalent damping of all actuators, JiAnd Bi(i ═ 1,2, …, n) denotes the equivalent moment of inertia and equivalent damping of the actuator i, respectively;
s104, calculating the integral value of the angular velocity of the actuator
Figure FDA0002439996000000021
The integral value
Figure FDA0002439996000000022
Comprises the following steps:
Figure FDA0002439996000000023
wherein the content of the first and second substances,
Figure FDA0002439996000000024
for the actuator angular velocity at time k read by the sensor,
Figure FDA0002439996000000025
the integral value of the angular speed of the actuator at the moment k;
s105, calculating the control quantity output to the actuator driver of the actuator i at the moment k by utilizing the following relation;
Figure FDA0002439996000000026
wherein, K1And K2The gains for position and velocity, respectively, are constants greater than 0.
2. A haptic interaction control method based on velocity sensor and differentiator force according to claim 1, wherein the integrated value of the angular velocity of the actuator is calculated
Figure FDA0002439996000000027
The method comprises the following steps:
calculating integral value of angular velocity of actuator by using Euler method
Figure FDA0002439996000000028
3. A haptic interaction control method based on a speed sensor and a differentiator force according to claim 1, further comprising:
judging whether to finish the control, if so, jumping out of the dead cycle in a mode of setting a flag bit, and finishing the control; if not, the flag bit is not set, and the next timer interruption is waited.
4. The haptic interaction control method based on the speed sensor and the differentiator force as claimed in claim 1, wherein the judging whether to end the control is performed, if yes, jumping out of a dead loop by setting a flag bit, and ending the control; if not, not setting the flag bit, and waiting for the next timer interrupt, further comprising:
and receiving sensor data sent by other control circuits or sending sensor data to other control circuits.
5. The haptic interaction control method based on speed sensor and differentiator force according to claim 1, wherein before processing the angular velocity of the actuator at time k of the discrete differentiator with feedforward and obtaining the angular acceleration of the actuator, further comprising:
and judging the type of the sensor, and executing the subsequent steps if the type of the sensor is a speed sensor.
6. A haptic interaction control method based on velocity sensor and differentiator force according to claim 1, wherein the filter is a first order low pass filter, a second order higher order low pass filter, a butterworth filter or an IIR digital filter.
7. The haptic interaction control method based on speed sensor and differentiator force according to claim 1, wherein the speed sensor employs a tachogenerator.
8. A haptic interaction control device based on a velocity sensor and a differentiator force, the device comprising:
the presetting unit is used for defining an endless loop in the main function in advance, the endless loop judges whether to exit the loop by using a variable as a flag bit, the loop is interrupted once every fixed time by using a timer, the kth time entering of the timer interrupt service function is defined as the time k, and the time entering of the timer interrupt service function before the time k is called as the time k-1;
the processing unit is used for processing the angular velocity of the actuator at the moment k of the discrete differentiator with feedforward to obtain the angular acceleration of the actuator:
Figure FDA0002439996000000031
Figure FDA0002439996000000032
Figure FDA0002439996000000033
Figure FDA0002439996000000034
wherein, TsIs an interrupt period of the timer and is,
Figure FDA0002439996000000035
is the sensor data read by the speed sensor of actuator i at time k,
Figure FDA0002439996000000036
is the low pass filtered value of the velocity sensor reading corresponding to the actuator i at time k,
Figure FDA0002439996000000037
and
Figure FDA0002439996000000038
an intermediate variable for differential calculation of the angular velocity of the actuator i,
Figure FDA0002439996000000039
is the differentiator output for the velocity sensor reading at time k for actuator i,
Figure FDA00024399960000000310
and
Figure FDA00024399960000000311
the gains of the angular velocity differentiator of the actuator i are constants larger than 0;
a filter unit for outputting the control quantity u of the actuator driver to the actuator i at the time k-1i(k-1) filtering;
Figure FDA00024399960000000312
where g is the gain of the filter,
Figure FDA00024399960000000313
is calculated at the k momentU of (a)i(k-1) low pass filtered values;
a first calculating unit for calculating an intermediate variable τi(k) Of intermediate variable τi(k) Including the information that the actuator i receives external acting force at the moment k, and the intermediate variable taui(k) Comprises the following steps:
Figure FDA0002439996000000041
wherein J is max { J ═ J1,J2,...,JnIs the largest one of the equivalent moments of inertia of all actuators; b ═ max { B1,B2,...,BnIs the largest of the equivalent damping of all actuators, JiAnd Bi(i ═ 1,2, …, n) denotes the equivalent moment of inertia and equivalent damping of the actuator i, respectively;
a second calculation unit for calculating an integral value of the angular velocity of the actuator
Figure FDA0002439996000000042
The integral value
Figure FDA0002439996000000043
Comprises the following steps:
Figure FDA0002439996000000044
wherein the content of the first and second substances,
Figure FDA0002439996000000045
for the actuator angular velocity at time k read by the sensor,
Figure FDA0002439996000000046
the integral value of the angular speed of the actuator at the moment k;
the output unit is used for calculating and obtaining the control quantity output to the actuator driver of the actuator i at the moment k by utilizing the following relation;
Figure FDA0002439996000000047
wherein, K1And K2The gains for position and velocity, respectively, are constants greater than 0.
9. A haptic interaction control device based on speed sensor and differentiator force according to claim 8, wherein the first computing unit is specifically configured to:
calculating integral value of angular velocity of actuator by using Euler method
Figure FDA0002439996000000048
10. A haptic interaction control device based on a speed sensor and a differentiator force according to claim 8, further comprising:
the judging unit is used for judging whether to finish the control, if so, jumping out of the dead cycle in a mode of setting a flag bit, and finishing the control; if not, the flag bit is not set, and the next timer interruption is waited.
CN201811485955.5A 2018-12-06 2018-12-06 Haptic interaction control method and device based on speed sensor and differentiator force Active CN109597437B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811485955.5A CN109597437B (en) 2018-12-06 2018-12-06 Haptic interaction control method and device based on speed sensor and differentiator force

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811485955.5A CN109597437B (en) 2018-12-06 2018-12-06 Haptic interaction control method and device based on speed sensor and differentiator force

Publications (2)

Publication Number Publication Date
CN109597437A CN109597437A (en) 2019-04-09
CN109597437B true CN109597437B (en) 2020-07-21

Family

ID=65962150

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811485955.5A Active CN109597437B (en) 2018-12-06 2018-12-06 Haptic interaction control method and device based on speed sensor and differentiator force

Country Status (1)

Country Link
CN (1) CN109597437B (en)

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH0623680A (en) * 1991-06-14 1994-02-01 Honeywell Inc Speed mode hand controller accompanied by reactive force
EP3254917A1 (en) * 2016-06-07 2017-12-13 Giuseppe Barelli Method and device to control vehicle behaviour
CN108748147A (en) * 2018-06-01 2018-11-06 清华大学深圳研究生院 A kind of control system and method for ectoskeleton mechanical arm

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2006163206A (en) * 2004-12-09 2006-06-22 Ntt Docomo Inc Tactile sense presentation device
CN102819272A (en) * 2012-08-24 2012-12-12 广东工业大学 Tele-manipulating robot force feedback control system and control method thereof
CN103309363B (en) * 2013-06-24 2015-12-02 中国科学院长春光学精密机械与物理研究所 A kind of bidirection control method revolved for scanning imaging system elimination picture

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH0623680A (en) * 1991-06-14 1994-02-01 Honeywell Inc Speed mode hand controller accompanied by reactive force
EP3254917A1 (en) * 2016-06-07 2017-12-13 Giuseppe Barelli Method and device to control vehicle behaviour
CN108748147A (en) * 2018-06-01 2018-11-06 清华大学深圳研究生院 A kind of control system and method for ectoskeleton mechanical arm

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Improvement of Force Transmission Performance Considering Nonlinear Friction in Bilateral Control Systems;Kenta Seki,Shota Fujihara,Makoto Iwasaki;《IFAC-Papers Online》;20170731;全文 *
力触觉反馈系统的虚拟现实应用研究;付志宇;《中国优秀硕士学位论文全文数据库 信息科技辑》;20151115;全文 *

Also Published As

Publication number Publication date
CN109597437A (en) 2019-04-09

Similar Documents

Publication Publication Date Title
CN106020124B (en) Servo motor control device and collision detection method
JP4972813B2 (en) Addressing device and method and converter
CN109803088A (en) Cloud platform control method, device and holder
US6046564A (en) Path planning apparatus and method for robots
Hinterseer et al. Perception-based compression of haptic data streams using kalman filters
CN113305839B (en) Admittance control method and admittance control system of robot and robot
EP3397431B1 (en) Robot and method for operating a robot
EP1493127A1 (en) Modular system for processing an image and software kit
CN110090443A (en) Virtual object control method and device, storage medium and electronic device
JP2000052286A (en) Robot control device
CN109597437B (en) Haptic interaction control method and device based on speed sensor and differentiator force
CN110072679A (en) Collision monitoring to robot
CN111753982B (en) Man-machine fusion autonomous boundary switching method and system based on reinforcement learning
EP1074899A1 (en) Controller and control method
CN109857162B (en) Force and touch interaction control method and device
EP1672322B1 (en) Reconfigurable input/output interface
CN110569767A (en) Motion parameter identification method, system, equipment and computer readable storage medium
CN109571475B (en) Force touch control system and robot
Aksman et al. Force estimation based compliance control of harmonically driven manipulators
JP3362379B2 (en) Method of detecting force in force control device
CN112583322A (en) Active-disturbance-rejection control method for linear motor and related device
EP0413030A1 (en) Servo control method by the disturbance estimation observer
KR20190126927A (en) Method, switching unit, device and system for data transfer from device to data management means
CN109768738B (en) Motor control device, control system, and motor control method
JP3871030B2 (en) Servo control method

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