CN113103263A - Three-degree-of-freedom rotary hand controller and control method thereof - Google Patents

Three-degree-of-freedom rotary hand controller and control method thereof Download PDF

Info

Publication number
CN113103263A
CN113103263A CN202110377517.2A CN202110377517A CN113103263A CN 113103263 A CN113103263 A CN 113103263A CN 202110377517 A CN202110377517 A CN 202110377517A CN 113103263 A CN113103263 A CN 113103263A
Authority
CN
China
Prior art keywords
axis
degree
hand controller
tarposition
bearing box
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
Application number
CN202110377517.2A
Other languages
Chinese (zh)
Other versions
CN113103263B (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.)
Nanjing University of Aeronautics and Astronautics
Shanghai Aerospace System Engineering Institute
Original Assignee
Nanjing University of Aeronautics and Astronautics
Shanghai Aerospace System Engineering Institute
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 Nanjing University of Aeronautics and Astronautics, Shanghai Aerospace System Engineering Institute filed Critical Nanjing University of Aeronautics and Astronautics
Priority to CN202110377517.2A priority Critical patent/CN113103263B/en
Publication of CN113103263A publication Critical patent/CN113103263A/en
Application granted granted Critical
Publication of CN113103263B publication Critical patent/CN113103263B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00—Controls for manipulators
    • B25J13/02—Hand grip control means
    • B25J13/025—Hand grip control means comprising haptic means
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
    • A61B34/70—Manipulators specially adapted for use in surgery
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
    • A61B34/70—Manipulators specially adapted for use in surgery
    • A61B34/76—Manipulators having means for providing feel, e.g. force or tactile feedback
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00—Controls for manipulators
    • B25J13/02—Hand grip control means

Landscapes

  • Engineering & Computer Science (AREA)
  • Health & Medical Sciences (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Robotics (AREA)
  • Surgery (AREA)
  • Medical Informatics (AREA)
  • Biomedical Technology (AREA)
  • Heart & Thoracic Surgery (AREA)
  • Nuclear Medicine, Radiotherapy & Molecular Imaging (AREA)
  • Molecular Biology (AREA)
  • Animal Behavior & Ethology (AREA)
  • General Health & Medical Sciences (AREA)
  • Public Health (AREA)
  • Veterinary Medicine (AREA)
  • Mechanical Engineering (AREA)
  • Human Computer Interaction (AREA)
  • Manipulator (AREA)

Abstract

The invention discloses a three-degree-of-freedom rotary hand controller and a control method thereof, aiming at providing a three-degree-of-freedom rotary hand controller with simple structure, convenient operation and low cost; the three-degree-of-freedom hand controller comprises: the three-freedom-degree rotating hand controller comprises a first driving device, a second driving device, a third driving device, an operating handle, a force sensor and a cross hinge, wherein the first driving device controls the device to rotate on a Z axis and provide force feedback, the second driving device controls the device to rotate on an X axis and provide force feedback, the third driving device controls the device to rotate on a Y axis and provide force feedback, and a user can control the three-freedom-degree rotating hand controller through controlling the operating handle, so that the whole operating process is visual and convenient.

Description

Three-degree-of-freedom rotary hand controller and control method thereof
Technical Field
The invention relates to a robot, in particular to a three-degree-of-freedom rotary hand controller and a control method thereof.
Background
The three-degree-of-freedom rotary hand controller is an operating mechanism widely applied to the fields of medical operation robot operation, robot remote control operating systems, force feedback control devices, virtual simulation interactive operation and the like, and can enable a user to operate a target device more intuitively, so that the friendliness of human-computer interaction is greatly improved, and the operating threshold of the user is reduced; however, at present, domestic research on the field of three-degree-of-freedom rotary hand controllers is still in a starting stage, the domestic three-degree-of-freedom rotary hand controllers have some disadvantages compared with the same type of products abroad, and domestic use of the three-degree-of-freedom rotary hand controllers still depends on import to a great extent; meanwhile, the three-degree-of-freedom rotary hand controller is complex in structure and expensive in price, and is not beneficial to reducing cost of a user.
Disclosure of Invention
The invention aims to research and develop a mechanical structure of a hand controller with a more exquisite structure and a matched command processing and issuing algorithm aiming at the defects of the existing three-degree-of-freedom rotary hand controller, and has the advantages of exquisite and simple structure, convenient installation, easy operation and low cost.
In order to achieve the purpose of the invention, the invention adopts the following technical scheme:
the utility model provides a three degree of freedom rotation hand controller, its characterized in that includes first drive arrangement, second drive arrangement, third drive arrangement, operating handle, force transducer, ring flange and cross hinge support, and the ring flange is fixed on the operation panel, and force transducer installs on the ring flange, and the sleeve is passed through to first drive arrangement lower extreme and installs on force transducer, and first drive arrangement includes: the X-axis universal joint comprises a Z-axis motor, a sleeve, a Z-axis coupler and a Z-axis bearing box, wherein the sleeve is surrounded outside the Z-axis motor, the power output end of the Z-axis motor is connected with one end of the Z-axis coupler, the Z-axis bearing box is sleeved outside the Z-axis coupler, the Z-axis bearing box is connected with the top end of the sleeve, the other end of the Z-axis coupler is connected with a joint fork rod of an X-axis joint fork, one joint fork head of the X-axis joint fork is sleeved on an X-axis second bearing box, the X-axis second bearing box is installed at the D end of a cross-shaped hinged support, the other joint fork head of the X-axis joint fork is sleeved on an X-axis first bearing box, and the X-axis first bearing; the enabling button is installed on an operating handle, and the operating handle is fixedly connected with the Y-axis joint fork through a handle rod.
Furthermore, A, B, C, D ends are arranged at four ends of the cross-shaped hinged support clockwise, A, B, C, D ends are all stepped shafts, the A end of the cross-shaped hinged support is connected with the Y-axis joint fork, and the B end of the cross-shaped hinged support is connected with the second driving device and the X-axis joint fork; the C end of the cross-shaped hinged support is connected with a third driving device and a Y-axis joint fork; the D end of the cross hinged support is connected with the X-axis yoke.
Furthermore, the second driving device comprises an X-axis motor, an X-axis first bearing box and an X-axis coupler, the power output end of the X-axis motor is connected with the B end of the cross hinge support through the X-axis coupler, the X-axis first bearing box is sleeved outside the X-axis coupler, and a fork head of the X-axis yoke is sleeved outside the X-axis first bearing box.
Furthermore, the third driving device comprises a Y-axis motor, a Y-axis first bearing box and a Y-axis coupler, the power output end of the Y-axis motor is connected with the C end of the cross hinge support through the Y-axis coupler, the Y-axis first bearing box is sleeved outside the Y-axis coupler, a fork head of a Y-axis joint fork is sleeved outside the Y-axis first bearing box, the other fork head of the Y-axis joint fork is sleeved on the Y-axis second bearing box, and the Y-axis second bearing box is installed at the A end of the cross hinge support.
Further, the force sensor is a six-position force sensor.
Furthermore, a protective cover is arranged on the outer side of the three-degree-of-freedom rotary hand controller.
Furthermore, the enabling button is installed on the operating handle through threaded connection, a threaded blind hole is formed in the operating handle in the axial direction, a threaded hole for installing the enabling button is formed in the side face of the operating handle, and the threaded blind hole in the operating handle is communicated with the threaded hole in the side face; the threaded blind hole of the operating handle is matched with the external thread at one end of the handle rod, the operating handle is fixedly connected with one end of the handle rod through threads, and the other end of the handle rod is fixedly connected with the middle part of the Y-axis yoke through threads; the handle rod is sequentially provided with an organ shield bearing and a shield bearing on the circumference of the outer side of the handle rod from the operating handle to the Y-axis joint fork, and the handle rod and the shield are connected together through the shield bearing.
Furthermore, the small opening end of the tower-shaped organ protective cover is connected with the bearing of the organ protective cover, and the large opening end of the tower-shaped organ protective cover is connected with the protective cover through screws.
Furthermore, a counterweight device is mounted on a joint fork head of the Y-axis joint fork connected with the A end of the cross-shaped hinged support, and the weight of the counterweight device is equal to that of the second driving device.
The invention also provides a control method based on the three-degree-of-freedom rotary hand controller, which comprises the following steps of:
step one, a power supply is switched on to open the three-degree-of-freedom rotary hand controller, and the current position is used as the initial position of the handle, so that the initialization of the operating handle is completed;
step two, a force sensor at the bottom of the three-freedom-degree rotating hand controller reads the torsion tau in the X, Y and Z directionsx、τy、τzThe read force values are leveled by a filtering algorithmPerforming sliding processing, and calculating to obtain a coordinate tarposition of the target position of the motor on the X axis through a force control algorithmxCoordinate on the Y-axis tarpositionyCoordinate tarposition on Z-axiszAnd the force feedback is transmitted to three motors on the corresponding three joints to enable the motors to generate force in the direction opposite to the direction of the force applied to the handle by a user, so that the effect of force feedback is achieved, namely
tarpositionx＝Kpx·τx
tarpositiony＝Kpy·τy
tarpositionz＝Kpz·τz
Wherein the proportionality coefficient Kpx、Kpy、KpzThe ratio of the expected torque to the corresponding motor pulse when the handle rotates to the limit position;
step three, smoothing the target position to prevent the handle from shaking, wherein the period is 10ms and delta ispx.py.pzRepresents the maximum acceptable motor position difference between two cycles, and the position difference value is within +/-deltapx.py.pzThe inner motor moves smoothly, and the position difference value is +/-deltapx.py.pzStrong shaking of the motion pause of the external motor is obvious, deltapx.py.pzThe value needs to be adjusted to observe the actual use effect in the use process, so as to determine the more ideal deltapx.py.pzValue if the difference between the target position of the previous calculation cycle and the target position of the present calculation cycle is less than the maximum value deltapx.py.pzI.e. by
|tarpositionx，y，z-tarposition′x，y，z|<Δpx.py.pz
The target position of the present calculation cycle remains unchanged, wherein tarpositionx,y，zIs the current target location, tarposotion'x，y，zCalculating a periodic target position for the previous time;
if the difference between the target position of the previous calculation period and the target position of the present calculation period is larger than the maximum value deltapx.py.pzI.e. by
|tarpositionx，y，z-tarposition′x，y，z|>Δpx.py.pz
The target position of the current calculation period is equal to the target position of the last calculation period plus or minus deltapx.py.pzI.e. by
tarpositionx.y.z＝tarposition′x，y，z±Δpx.py.pz；
Step four, giving a speed instruction to the handle control robot, and performing acceleration and deceleration planning on the Cartesian space attitude of the robot, wherein the delta is deltavx.vy.vzRepresents the maximum acceptable speed command difference between two cycles, and the speed command difference is within +/-deltavx.vy.vzWithin the range, the controlled robot moves smoothly, and the speed command difference is +/-deltavx.vy.vzOut of range, the controlled robot may shake during movement, Δvx.vy.vzThe value needs to be adjusted to observe the actual use effect in the use process, so as to determine the more ideal deltavx.vy.vzIf the difference between the speed command of the current period and the speed command of the previous period is less than deltavx.vy.vzI.e. by
|Vx，y，z-V′x，y，z|<Δvx.vy.vz
The present cycle speed command remains unchanged, where Vx,y,zIs the current speed command speed, V'x,y,zThe speed command speed is calculated for the last cycle.
If the difference between the speed command of the current period and the speed command of the previous period is larger than deltavx.vy.vzI.e. by
|Vx，y，z-V′x，y，z|>Δvx.vy.vz
Then make the current cycle speed command equal to the last cycle speed command plus or minus deltavx.vy.vzI.e. by
Vx.y.z＝V′x，y，z±Δvx.vy.vz
Step five, carrying out speed limiting processing on the actual speed instruction, and enabling the actual speed instruction to be equal to the maximum speed when the actual speed instruction is greater than the maximum speed;
and step six, issuing the finally calculated actual speed instruction to a target device for control.
Compared with the prior art, the invention adopting the technical scheme has the following technical effects:
the three-degree-of-freedom rotary hand controller provided by the invention can identify the operation of an operator and give feedback on three degrees of freedom, so that the operator can use the hand controller in the fields of medical operation robot operation, robot remote control operation systems, force feedback control devices, virtual simulation interactive operation and the like, interactive feedback is generated in the operation process, and a user can operate more intuitively.
Drawings
FIG. 1 is a schematic view of the overall structure of a three-degree-of-freedom rotary hand controller according to the present invention;
FIG. 2 is a right side view of an assembly view of the three-degree-of-freedom rotary hand controller of the present invention;
FIG. 3 is a top view of an assembly diagram of the three-degree-of-freedom rotary hand controller of the present invention;
fig. 4 is a cross hinge bracket component diagram of the three-degree-of-freedom rotary hand controller of the present invention.
In the figure: 1-operating handle, 2-force sensor, 3-cross hinge support, 4-flange plate, 5-counterweight device, 6-enabling button, 7-Z shaft motor, 8-sleeve, 9-Z shaft bearing box, 10-X shaft yoke, 11-Z shaft coupler, 12-X shaft motor, 13-X shaft first bearing box, 14-X shaft coupler, 15-Y shaft motor, 16-Y shaft yoke, 17-Y shaft coupler, 18-protective cover, 19-tower type organ protective cover, 20-organ protective cover bearing, 21-X shaft second bearing box, 22-Y shaft first bearing box, 23-Y shaft bearing box, 24-handle rod and 25-protective cover bearing.
Detailed Description
The present invention will be further described with reference to the accompanying drawings.
As shown in fig. 1, 2 and 3, a three-degree-of-freedom rotary hand controller includes: the device comprises a first driving device, a second driving device, a third driving device, an operating handle 1, a force sensor 2 and a cross hinge support 3.
Whole hand controller passes through ring flange 4 to be installed on the operation panel, and 4 bottom surfaces of ring flange and operation mesa contact, install force sensor 2 above the ring flange 4, and force sensor 2 is connected with first drive arrangement.
As shown in fig. 4, the four ends of the cross-shaped hinge bracket 3 are clockwise A, B, C, D ends, the end a is a third-step shaft, the end B is a fourth-step shaft, the end C is a fourth-step shaft, and the end D is a third-step shaft.
As shown in fig. 2, the first driving means includes a Z-axis motor 7, a sleeve 8, the X-axis joint yoke 10 and the Z-axis coupler 11, a sleeve 8 surrounds a Z-axis motor 7, the lower end of the sleeve 8 is fixed on a force sensor 2 through a screw, the power output end of the Z-axis motor 7 is connected with one end of the Z-axis coupler 11, a Z-axis bearing box 9 is sleeved outside the Z-axis coupler 11, the Z-axis bearing box 9 is connected with the top end of the sleeve 8, the other end of the Z-axis coupler 11 is connected with a joint yoke rod of the X-axis joint yoke 10, two joint yoke heads are arranged at the front end of the X-axis joint yoke 10, one joint yoke of the X-axis joint yoke 10 is sleeved on an X-axis first bearing box 13 at the B end of the cross-shaped hinged support 3, an X-axis second bearing box 21 is installed at the D end of the cross-shaped hinged support 3, and the other joint yoke head of the X-axis joint yoke 10 is sleeved on the X-axis second bearing box 21, so that a first driving device is.
As shown in fig. 2 and 3, the second driving device includes an X-axis motor 12, an X-axis bearing box 13 and an X-axis coupler 14, a power output end of the X-axis motor 12 is connected with a B end of the cross-hinge bracket 3 through the X-axis coupler 14, the X-axis first bearing box 13 is sleeved outside the X-axis coupler 14, and a fork head of the X-axis yoke 10 is sleeved outside the X-axis first bearing box 13.
As shown in fig. 3, the third driving device includes a Y-axis motor 15, a Y-axis bearing box 16 and a Y-axis coupler 17, a power output end of the Y-axis motor 15 is connected with the C-end of the cross-hinge bracket 3 through the Y-axis coupler 17, a Y-axis first bearing box 22 is sleeved outside the Y-axis coupler 17, and a fork head of the Y-axis joint fork 16 is sleeved outside the Y-axis first bearing box 22.
The end A of the cross-shaped hinged support 3 is provided with a Y-axis second bearing box 23, the Y-axis second bearing box 23 is externally sleeved with the other fork head of the Y-axis joint fork 16, the fork head is welded with a counterweight device 5, and the weight of the counterweight device 5 is equal to that of the third driving device, so that the balance of the whole three-freedom-degree rotating hand controller in the Y-axis direction is ensured.
The first driving device controls the rotation of the three-freedom-degree rotary hand controller in the Z-axis direction, the second driving device controls the rotation of the three-freedom-degree rotary hand controller in the X-axis direction, the third driving device controls the rotation of the three-freedom-degree rotary hand controller in the Y-axis direction, and the three driving devices can control the target hand controller in three degrees of freedom.
The operating handle 1 is cylindrical, a threaded blind hole is formed in the end face of one end of the cylinder, a threaded hole is formed in the side face of the operating handle 1 and is perpendicular to and communicated with the threaded blind hole, and the enabling button 6 is installed in the threaded hole in the side face of the operating handle 1 through threaded connection.
The threaded blind hole on the end face of the operating handle 1 is matched with the external thread on one end of the handle rod 24, the operating handle 1 is fixed at one end of the handle rod 24 through threaded connection, the other end of the handle rod 24 is fixed at the middle part of the Y-axis joint fork 16 through threaded connection, a through hole is axially formed in the handle rod 24 and can be used for enabling a connecting wire of the button 6 to pass through, one end of the connecting wire of the button 6 is connected with the button 6, and the other end of the button 6 is connected with a driver bus.
The handle lever 24 is provided with an organ shield bearing 20 and a shield bearing 25 on the outer circumference of the handle lever 24 in order from the operating handle 1 to the Y-axis yoke 16, and the shield bearing 25 connects the handle lever 24 and the shield 18 together.
The small opening end of the tower-shaped organ shield 19 is connected with the organ shield bearing 20, and the large opening end of the tower-shaped organ shield 19 is connected with the shield 18 through screws.
The control method of the three-degree-of-freedom rotary hand controller comprises the following steps of:
step one, after a power supply is switched on to open the three-degree-of-freedom rotary hand controller, the enable button 6 on the operating handle 1 is double-clicked, and then the current position can be used as the initial position of the operating handle 1, so that the initialization of the operating handle 1 is completed.
Step two, when the user operates the operating handle 1, the force sensor 2 at the bottom of the three-freedom-degree rotating hand controller reads X and YTorsion t in three directions Zx、τy、τzSmoothing the read force value through a filtering algorithm, and calculating to obtain the coordinate tarposition on the X axis of the motor target position through a force control algorithmxCoordinate on the Y-axis tarpositionyCoordinate tarposition on Z-axiszAnd the force feedback is transmitted to three motors on the corresponding three joints to enable the motors to generate force in the direction opposite to the direction of the force applied to the handle by a user, so that the effect of force feedback is achieved, namely
tarpositionx＝Kpx·τx
tarpositiony＝Kpy·τy
tarpositionz＝Kpz·τz
Wherein the proportionality coefficient Kpx、Kpy、KpzThe ratio of the expected torque to the corresponding motor pulse when the handle rotates to the limit position;
step three, after the target position of the motor is obtained through calculation, smoothing is needed to be carried out on the target position to prevent the handle from shaking, 10ms is taken as a period, and delta is deltapx.py.pzRepresenting the maximum acceptable motor position difference, Δ, between two cyclespxRepresents the maximum acceptable motor position difference, Δ, on the X-axis between two cyclespyRepresents the maximum acceptable motor position difference, Δ, in the Y-axis between the two cyclespzRepresents the maximum acceptable motor position difference in the Z-axis between the two cycles; position difference value within +/-Deltapx.py.pzThe inner motor moves smoothly, and the position difference value is +/-deltapx.py.pzStrong shaking of the motion pause of the external motor is obvious, deltapx.py.pzThe value needs to be adjusted to observe the actual use effect in the use process, so as to determine the more ideal deltapx.py.pzValue if the difference between the target position of the previous calculation cycle and the target position of the present calculation cycle is less than the maximum value deltapx.py.pzI.e. by
|tarpositionx，y，z-tarposition′x，y，z|<Δpx.py.pz
The target position of the present calculation cycle remains unchanged, wherein tarpositionx，y，zIs the current target location, tarposotion'x，y，zCalculating a periodic target position for the previous time;
if the difference between the target position of the previous calculation period and the target position of the present calculation period is larger than the maximum value deltapx.py.pzI.e. by
|tarpositionx，y，z-tarposition′x，y，z|>Δpx.py.pz
The target position of the current calculation period is equal to the target position of the last calculation period plus or minus deltapx.py.pzI.e. by
tarpositionx.y.z＝tarposition′x，y，z±Δpx.py.pz
Wherein Δpx.py.pzThe motor parameter of the structure is taken as an empirical value.
After the target position is calculated, a speed instruction needs to be issued to the device to ensure the stability and reliability of control and prevent dangerous conditions such as galloping and the like of the device; the handle reads the current positions of the three motors and the current position X of the X axisposCurrent position Y of Y-axisposCurrent position Z of Z-axisposThereafter, the position is multiplied by a factor Kx、Ky、KzObtaining a speed command, X-axis direction speed VxSpeed V in Y-axis directionyZ-axis direction velocity VzI.e. by
Vx＝Xpos·Kx
Vy＝Ypos·Ky
Vz＝Zpos·Kz
Wherein, Kx、Ky、KzIs set by the user during use.
If the robot is controlled in cartesian space attitude (pitch, yaw, roll) by using the handle, after obtaining the velocity command, it is necessary to perform smoothing processing, i.e., acceleration and deceleration planning, on the velocity, and Δvx.vy.vzIndicating acceptability between two cyclesMaximum speed command difference, ΔvxIndicating the maximum acceptable speed command difference, Δ, in the X-axis direction between two cyclesvyIndicating the maximum acceptable speed command difference, Δ, in the Y-axis direction between two cyclesvzIndicating the maximum acceptable speed command difference in the Z-axis direction between the two cycles; speed command difference of deltavx.vy.vzWithin the range, the controlled robot moves smoothly, and the speed command difference is +/-deltavx.vy.vzOut of range, the controlled robot may shake during movement, Δvx.vy.vzThe value needs to be adjusted to observe the actual use effect in the use process, so as to determine the more ideal deltavx.vy.vzIf the difference between the speed command of the current period and the speed command of the previous period is less than deltavx.vy.vzI.e. by
|Vx，y,z-V′x，y，z|<Δvx.vy.vz
The present cycle speed command remains unchanged, where Vx，y，zIs the current speed command speed, V'x，y,zCalculating a cycle speed command speed for the last cycle;
if the difference between the speed command of the current period and the speed command of the previous period is larger than deltavx.vy.vzI.e. by
|Vx，y,z-V′x，y，z|>Δvx.vy.vz
Then make the current cycle speed command equal to the last cycle speed command plus or minus deltavx.vy.vzI.e. by
Vx.y.z＝V′x,y，z±Δvx.vy.vz
And fifthly, performing speed limiting processing on the actual speed instruction, and enabling the actual speed instruction to be equal to the maximum speed when the actual speed instruction is larger than the maximum speed.
And step six, after the final actual speed instruction is calculated through the algorithm, the actual speed instruction can be issued to the target device for control.
Example 1:
the invention provides a three-degree-of-freedom rotary hand controller and a control method thereofRotating towards the right hand side direction of a user, after the user rotates in place, reading that the current motor pulse position is 20000 by a motor corresponding to the rotating direction, and simultaneously reading the moment tau applied to the three-freedom-degree rotating handle by the user in the Z-axis direction by the user through a force sensorz0.5126N m.
After the force control algorithm obtains the moment parameter, the moment number tau is applied tozMultiplying by a force control parameter Kpz(KpzUser can set himself, in this embodiment set 212500) to obtain the target position tarposion sent to the Z-axis motorzI.e. tarpositionz＝Kpz·τz0.5126 × 212500 ═ 108927.5; using 10ms as a period, the moment of the previous period is 0.5118N × m, the target position of the previous period is 108757.5, and the difference between the target position of the previous calculation period and the target position of the present calculation period is smaller than the maximum value Δ p (Δ p is 200), that is, | tarposionz-tarposition′z|＝170<ΔpzTherefore, the target position can be sent to the Z-axis motor without being changed; tarposition of target positionzSent to a Z-axis motor which tries to rotate to a given target position tarposionzI.e. a force in the opposite direction to the force applied by the user to the handle, thereby creating a force feedback effect.
Meanwhile, the read current position 20000 pulses of the Z-axis motor are multiplied by a coefficient Kz(Kz0.00004706) calculates the velocity command V to be sent to the target robotzI.e. Vz＝Zpos·Kz20000 × 0.00004706 ═ 0.9412 °/s, 10ms is taken as a period, and V calculated from the previous periodz ′0.9306 DEG/s, the variation of the speed command in the current cycle and the previous cycle is Vz-Vz' -0.0106 °/s, greater than a maximum value Δ vz (Δ vz is 0.01 °/s), i.e. | Vz-Vz′|>ΔvzThen command the speed command V of this cyclezEqual to the upper cycle speed command plus Δ V, i.e. Vz＝Vz ′+Δvz0.9306+0.01 ═ 0.9406 °/s. Then judging whether the obtained speed command exceeds the maximum speed V or notmax(VmaxIs 2 °/s), i.e. Vz<VmaxTherefore, the speed command can be sent to the target robot to be controlled without further modification so as to complete the control of the target robot.

Claims (10)

1. The utility model provides a three degree of freedom rotation hand controller, its characterized in that includes first drive arrangement, second drive arrangement, third drive arrangement, operating handle (1), force transducer (2), ring flange (4) and cross hinge support (3), and ring flange (4) are fixed on the operation panel, and force transducer (2) are installed on ring flange (4), and the sleeve (8) is passed through to first drive arrangement lower extreme and installs on force transducer (2), and first drive arrangement includes: the X-axis hinge comprises a Z-axis motor (7), a sleeve (8), a Z-axis coupler (11) and a Z-axis bearing box (9), wherein the sleeve (8) surrounds the Z-axis motor (7), the power output end of the Z-axis motor (7) is connected with one end of the Z-axis coupler (11), the Z-axis bearing box (9) is sleeved outside the Z-axis coupler (11), the Z-axis bearing box (9) is connected with the top end of the sleeve (8), the other end of the Z-axis coupler (11) is connected with a joint fork rod of an X-axis joint fork (10), one joint fork head of the X-axis joint fork (10) is sleeved on the X-axis second bearing box (21), the X-axis second bearing box (21) is installed at the D end of a cross hinge support (3), the other joint fork head of the X-axis joint fork (10) is sleeved on the X-axis first bearing box (13), and the X-axis first bearing box (13) is installed at the B end of the cross hinge support (3); the enabling button (6) is installed on the operating handle (1), and the operating handle (1) is fixedly connected with the Y-axis yoke (16) through a handle rod (24).
2. The three-degree-of-freedom rotary hand controller according to claim 1, wherein the four ends of the cross-shaped hinged support (3) are arranged A, B, C, D ends clockwise, A, B, C, D ends are all stepped shafts, the end a of the cross-shaped hinged support (3) is connected with the Y-axis yoke (16), and the end B of the cross-shaped hinged support (3) is connected with the second driving device and the X-axis yoke (10); the C end of the cross-shaped hinged support (3) is connected with a third driving device and a Y-axis yoke (16); the D end of the cross hinge support (3) is connected with an X-axis joint fork (10).
3. The three-degree-of-freedom rotary hand controller according to claim 1, wherein the second driving device comprises an X-axis motor (12), an X-axis first bearing box (13) and an X-axis coupler (14), a power output end of the X-axis motor (12) is connected with a B end of the cross hinge bracket (3) through the X-axis coupler (14), the X-axis first bearing box (13) is sleeved outside the X-axis coupler (14), and a fork head of the X-axis yoke (10) is sleeved outside the X-axis first bearing box (13).
4. The three-degree-of-freedom rotary hand controller according to claim 1, wherein the third driving device comprises a Y-axis motor (15), a Y-axis first bearing box (22) and a Y-axis coupler (17), a power output end of the Y-axis motor (15) is connected with a C end of the cross hinge support (3) through the Y-axis coupler (17), the Y-axis first bearing box (22) is sleeved outside the Y-axis coupler (17), a fork head of the Y-axis yoke (16) is sleeved outside the Y-axis first bearing box (22), another fork head of the Y-axis yoke (16) is sleeved on the Y-axis second bearing box (23), and the Y-axis second bearing box (23) is installed at an a end of the cross hinge support (3).
5. The three-degree-of-freedom rotary hand controller of any one of claim 1, wherein the force sensor (2) is a six-position force sensor.
6. The three-degree-of-freedom rotary hand controller according to claim 1, wherein there is a protective cover (18) outside the three-degree-of-freedom rotary hand controller.
7. The three-degree-of-freedom rotary hand controller according to claim 1, wherein the enable button (6) is mounted on the operating handle (1) through threaded connection, the operating handle (1) is axially provided with a threaded blind hole, the side surface of the operating handle (1) is provided with a threaded hole for mounting the enable button (6), and the threaded blind hole on the operating handle (1) is communicated with the threaded hole on the side surface; the thread blind hole of the operating handle (1) is matched with the external thread at one end of the handle rod (24), the operating handle (1) is fixedly connected with one end of the handle rod (24) through threads, and the other end of the handle rod (24) is fixedly connected with the middle part of the Y-axis yoke (16) through threads; an organ shield bearing (20) and a shield bearing (25) are sequentially arranged on the outer circumference of the handle rod (24) from the direction from the operating handle (1) to the Y-axis joint fork (16), and the handle rod (24) and the shield (18) are connected together by the shield bearing (25).
8. The three-degree-of-freedom rotary hand controller according to claim 7, wherein the small opening end of the tower-shaped organ shield (19) is connected with the organ shield bearing (20), and the large opening end of the tower-shaped organ shield (19) is connected with the shield (18) through screws.
9. The three-degree-of-freedom rotary hand controller according to claim 1, wherein a counterweight device (5) is mounted on a yoke head of the Y-axis yoke (16) connected to the a end of the cross-shaped hinge support (3), and the weight of the counterweight device (5) is equal to the weight of the second driving device.
10. The method for controlling a three-degree-of-freedom rotary hand controller as claimed in any one of claims 1 to 9, comprising the steps of:
step one, a power supply is switched on to open the three-degree-of-freedom rotary hand controller, and the current position is used as the initial position of the operating handle (1), so that the initialization of the operating handle (1) is completed;
step two, a force sensor (2) at the bottom of the three-freedom-degree rotating hand controller reads the torsion tau in the X, Y and Z directionsx、τy、τzSmoothing the read force value through a filtering algorithm, and calculating to obtain the coordinate tarposition on the X axis of the motor target position through a force control algorithmxCoordinate on the Y-axis tarpositionyCoordinate tarposition on Z-axiszAnd the force feedback is transmitted to three motors on the corresponding three joints to enable the motors to generate force with the direction opposite to the force applied to the operating handle (1) by a user, so that the effect of force feedback is achieved, namely the force feedback
tarpositionx＝Kpx·τx
tarpositiony＝Kpy·τy
tarpositionz＝Kpz·τz
Wherein the proportionality coefficientKpx、Kpy、KpzThe ratio of the expected torque to the corresponding motor pulse when the handle rotates to the limit position;
step three, smoothing the target position to prevent the handle from shaking, wherein the period is 10ms and delta ispx.py.pzRepresents the maximum acceptable motor position difference between two cycles, and the position difference value is within +/-deltapx.py.pzThe inner motor moves smoothly, and the position difference value is +/-deltapx.py.pzStrong shaking of the motion pause of the external motor is obvious, if the difference between the target position of the last calculation period and the target position of the present calculation period is less than the maximum value deltapx.py.pzI.e. by
|tarpositionx,y,z-tarposition′x,y,z|<Δpx.py.pz
The target position of the present calculation cycle remains unchanged, wherein tarpositionx，y，zIs the current target location, tarposotion'x,y，zCalculating a periodic target position for the previous time;
if the difference between the target position of the previous calculation period and the target position of the present calculation period is larger than the maximum value deltapx.py.pzI.e. by
|tarpositionx,y,z-tarposition′x，y，z|>Δpx.py.pz
The target position of the current calculation period is equal to the target position of the last calculation period plus or minus deltapx.py.pzI.e. by
tarpositionx.y.z＝tarposition′x，y，z±Δpx.py.pz；
Step four, giving a speed instruction to the handle control robot, and performing acceleration and deceleration planning on the Cartesian space attitude of the robot, wherein the delta is deltavx.vy.vzRepresents the maximum acceptable speed command difference between two cycles, and the speed command difference is within +/-deltavx.vy.vzWithin the range, the controlled robot moves smoothly, and the speed command difference is +/-deltavx.vy.vzOut of range, the controlled robot may shake during movement, if the difference between the speed command of the current period and the speed command of the previous period is less than deltavx.vy.vzI.e. by
|Vx,y，z-V′x,y,z|<Δvx.vy.vz
The present cycle speed command remains unchanged, where Vx,y,zIs the current speed command speed, V'x,y，zCalculating a cycle speed command speed for the last cycle;
if the difference between the speed command of the current period and the speed command of the last calculation period is larger than deltavx.vy.vzI.e. by
|Vx，y，z-V′x，y，z|>Δvx.vy.vz
Then make the current cycle speed command equal to the last calculation cycle speed command plus or minus deltavx.vy.vzI.e. by
Vx.y.z＝V′x，y,z±Δvx.vy.vz；
Step five, carrying out speed limiting processing on the actual speed instruction, and enabling the actual speed instruction to be equal to the maximum speed when the actual speed instruction is greater than the maximum speed;
and step six, issuing the finally calculated actual speed instruction to a target device for control.
CN202110377517.2A 2021-04-08 2021-04-08 Three-degree-of-freedom rotary hand controller and control method thereof Active CN113103263B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110377517.2A CN113103263B (en) 2021-04-08 2021-04-08 Three-degree-of-freedom rotary hand controller and control method thereof

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110377517.2A CN113103263B (en) 2021-04-08 2021-04-08 Three-degree-of-freedom rotary hand controller and control method thereof

Publications (2)

Publication Number Publication Date
CN113103263A true CN113103263A (en) 2021-07-13
CN113103263B CN113103263B (en) 2021-12-07

Family

ID=76714707

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110377517.2A Active CN113103263B (en) 2021-04-08 2021-04-08 Three-degree-of-freedom rotary hand controller and control method thereof

Country Status (1)

Country Link
CN (1) CN113103263B (en)

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101439515A (en) * 2008-12-25 2009-05-27 上海交通大学 Parallel four-bar type structure decoupling six-dimension force feedback device
CN103406908A (en) * 2013-07-31 2013-11-27 北京邮电大学 Force feedback hand controller with three degrees of freedom rotation
CN105108762A (en) * 2015-09-02 2015-12-02 吉林大学 Remote control hand controller based on force/touch guiding
CN205704267U (en) * 2016-05-11 2016-11-23 华南理工大学 One series parallel type 6DOF force feedback manipulator
CN107067921A (en) * 2016-09-28 2017-08-18 哈尔滨理工大学 A kind of force feedback apparatus that can be extended to seven freedom
CN112405501A (en) * 2020-11-13 2021-02-26 重庆邮电大学 Series-parallel six-freedom-degree force feedback device with differential gear train

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101439515A (en) * 2008-12-25 2009-05-27 上海交通大学 Parallel four-bar type structure decoupling six-dimension force feedback device
CN103406908A (en) * 2013-07-31 2013-11-27 北京邮电大学 Force feedback hand controller with three degrees of freedom rotation
CN105108762A (en) * 2015-09-02 2015-12-02 吉林大学 Remote control hand controller based on force/touch guiding
CN205704267U (en) * 2016-05-11 2016-11-23 华南理工大学 One series parallel type 6DOF force feedback manipulator
CN107067921A (en) * 2016-09-28 2017-08-18 哈尔滨理工大学 A kind of force feedback apparatus that can be extended to seven freedom
CN112405501A (en) * 2020-11-13 2021-02-26 重庆邮电大学 Series-parallel six-freedom-degree force feedback device with differential gear train

Also Published As

Publication number Publication date
CN113103263B (en) 2021-12-07

Similar Documents

Publication Publication Date Title
US9037293B2 (en) Robot
JP5895628B2 (en) ROBOT CONTROL METHOD, ROBOT CONTROL DEVICE, AND ROBOT CONTROL SYSTEM
US20160161937A1 (en) Robot and robot controller
EP2703130A2 (en) Robot
WO2003068464A1 (en) Drive control method and drive controller
WO2003086718A1 (en) Robot
EP3650298B1 (en) System for integrated auto-steering and auto-braking mechanism in autonomous vehicles as a retro fit
CN103406908A (en) Force feedback hand controller with three degrees of freedom rotation
CN105171725A (en) Joint robot with motion assisting device
CN113103263B (en) Three-degree-of-freedom rotary hand controller and control method thereof
JPH02256483A (en) Speed control device for industrial robot
Kim et al. Remotely manipulated peg-in-hole task conducted by cable-driven parallel robots
CN208592849U (en) A kind of seven axis joint robots based on hollow driver
JP2020015124A (en) Robot control method, article manufacturing method, robot control device, robot, program and recording medium
JP4842561B2 (en) Force controller device
US20230052996A1 (en) Method of obtaining vibrational properties of robot arm
WO1995002487A1 (en) Manipulator
KR101474778B1 (en) Control device using motion recognition in artculated robot and method thereof
KR20050048902A (en) Haptic system using voice coil motor
JP2017164833A (en) Control device of manipulator device, control method of manipulator device, and control program of manipulator device
CN113093549B (en) Compound control method of multi-axis numerical control equipment
JP6238110B2 (en) Robot hand control method and control device
JP3151437B2 (en) Position control method and position control device thereof
JPH079608B2 (en) Robot control method and apparatus
WO2020240846A1 (en) Control device, control method, and robot system

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