WO2024021599A1 - 一种主从手机器人的控制方法及装置 - Google Patents

一种主从手机器人的控制方法及装置 Download PDF

Info

Publication number
WO2024021599A1
WO2024021599A1 PCT/CN2023/078377 CN2023078377W WO2024021599A1 WO 2024021599 A1 WO2024021599 A1 WO 2024021599A1 CN 2023078377 W CN2023078377 W CN 2023078377W WO 2024021599 A1 WO2024021599 A1 WO 2024021599A1
Authority
WO
WIPO (PCT)
Prior art keywords
slave
motor
speed
cycle
hand
Prior art date
Application number
PCT/CN2023/078377
Other languages
English (en)
French (fr)
Inventor
孙毅
钟鹏飞
骆威
Original Assignee
以诺康医疗科技 (苏州) 有限公司
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 以诺康医疗科技 (苏州) 有限公司 filed Critical 以诺康医疗科技 (苏州) 有限公司
Publication of WO2024021599A1 publication Critical patent/WO2024021599A1/zh

Links

Classifications

    • 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/30—Surgical robots
    • A61B34/37—Master-slave robots
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J3/00—Manipulators of master-slave type, i.e. both controlling unit and controlled unit perform corresponding spatial movements
    • 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

Definitions

  • the present application relates to the field of robot technology, and in particular to a control method and device for a master-slave robot.
  • Human hands often have certain limitations when performing small-scale operations, such as hand tremors and other subtle hand movements.
  • master-slave robots that can replace part of the work of human hands have emerged.
  • the master-slave robot mainly includes a master hand and a slave hand corresponding to the master hand.
  • the master hand is completely controlled by the operator and truthfully reflects the movement parameters of the operator's hand.
  • the slave hand is usually a mechanical arm, and the robot arm is equipped with a At least one joint is provided with a motor on each joint. In actual control, the motion parameters of each motor of the slave hand are mapped based on the three-dimensional coordinates of the master hand (usually the set point on the master hand).
  • the motion of the master hand is transmitted through the mechanical structure mapping of the master-slave robot.
  • the target setting point (such as the end point) of the slave hand is finally made to follow the master hand in the corresponding movement.
  • the motion parameters of each motor of the slave hand are directly mapped according to the motion of the master hand.
  • the acceleration generated in some cases such as: rapidly moving in the first direction
  • a sudden reverse acceleration movement during movement may exceed the limit rotation acceleration of the slave motor after being mapped to the slave motor, causing the motor to report an error, causing the slave hand to be unable to accurately follow the master hand in movement, or the life of the slave hand's motor to be affected.
  • embodiments of the present application provide a method for controlling a master-slave robot.
  • the slave hand of the master-slave robot is provided with at least one slave motor.
  • the method is used to control the movement of the slave motor; the method includes:
  • the mapping position is used to represent the number of mapping rotation pulses, which is directly mapped based on the three-dimensional coordinates of the master hand and the mechanical structure of the master-slave robot.
  • the three-dimensional coordinates of the master hand are based on the preset Obtained by collection cycle, the current cycle is any collection cycle;
  • the initial motion parameters include the initial speed V s and the initial acceleration A s ;
  • the value that can be reached first among the mapping movement distance and the maximum planning distance P cm is determined as the target distance; the mapping movement distance is the current position of the slave mobile phone.
  • the actual position is used to represent the actual number of rotation pulses.
  • the previous cycle is the previous acquisition cycle of the current cycle;
  • the sum of the actual position of the slave handheld motor in the previous cycle and the distance to the target is determined as the actual position of the slave handheld motor in the current cycle.
  • an implementable manner of the first aspect also includes:
  • the slave hand motor is controlled to move to the actual position of the current cycle.
  • an implementable manner of the first aspect also includes:
  • the corrected position of the slave hand motor in the current cycle is obtained based on the limit coordinates of the slave hand;
  • the method further includes:
  • the method of inverse kinematics solution is used to obtain each slave hand.
  • the three-dimensional coordinates of the main hand in the current period are corrected to the limit coordinates of the main hand;
  • the mapped position of the slave hand motor in the current cycle is reacquired until the actual position of the slave hand motor in the current cycle is redetermined.
  • an implementable manner of the first aspect also includes:
  • the three-dimensional coordinates of the main hand in the current cycle are corrected to the target three-dimensional coordinates of the main hand;
  • the mapping position of the slave hand motor in the current cycle is reacquired until the actual position of the slave hand motor in the current cycle is redetermined.
  • obtaining the initial motion parameters of the mobile phone motor in the current cycle includes:
  • the initial speed V s and initial acceleration A s are determined.
  • the initial velocity V s and the initial acceleration A s are determined based on the actual position of the slave mobile motor in the three acquisition cycles before the current cycle, including:
  • the first actual position is the actual position of the slave handheld motor in the previous cycle.
  • the second actual position is the actual position of the slave mobile phone in the previous acquisition cycle of the previous cycle;
  • the third actual position is the slave mobile motor in the current The actual position of the third acquisition cycle before the cycle;
  • the first average speed is determined as the initial speed V s
  • the average acceleration is determined as the initial acceleration A s .
  • the method further includes:
  • the movement trajectory of the slave handheld motor in the current cycle is planned according to the initial movement parameters, include:
  • the initial speed V s is equal to the limit speed V m , then for the movement trajectory of the slave mobile phone in the current period, it is planned to move at a constant speed according to the limit speed V m to the end of the current period.
  • an implementable manner of the first aspect also includes:
  • the initial acceleration A s is equal to the limit acceleration A m , determine the first time required to uniformly accelerate from the initial speed V s to the limit speed V m according to the limit acceleration A m ;
  • the plan is to first uniformly accelerate to the limit speed V m according to the limit acceleration A m , and then in the remaining time of the current period, according to the limit speed V m m moves at a constant speed until the end of the current period.
  • an implementable manner of the first aspect also includes:
  • the plan is to uniformly accelerate the movement from the initial speed V s to the end of the current period according to the limit acceleration Am .
  • an implementable manner of the first aspect also includes:
  • the plan is to first accelerate to the limit acceleration A m according to the first jerk, and then accelerate uniformly according to the limit acceleration A m to the limit speed V m , and finally move at a constant speed according to the limit speed V m during the remaining time of the current period until the end of the current period.
  • an implementable manner of the first aspect also includes:
  • the sum of the second duration and the third duration is greater than the duration of the current period, then for the motion trajectory of the slave mobile phone in the current period, it is planned to first accelerate from the initial acceleration A s to the limit acceleration A m , and then in the remaining time of the current period , uniformly accelerating according to the limit acceleration A m to the end of the current period.
  • an implementable manner of the first aspect also includes:
  • the plan is to accelerate uniformly from the initial speed V s to the limit speed V m according to the first jerk, and then accelerate it uniformly from the initial speed V s to the limit speed V m during the remaining time of the current cycle. , Move at a constant speed according to the limit speed V m until the end of the current period.
  • an implementable manner of the first aspect also includes:
  • the plan is to accelerate uniformly from the initial speed V s according to the first jerk to the end of the current cycle.
  • an implementable manner of the first aspect also includes:
  • the plan is to first accelerate uniformly from the initial speed V s to the limit speed V m according to the first jerk, and then continue the rest of the current cycle. Within, it moves at a constant speed according to the limit speed V m until the end of the current period.
  • obtaining the maximum planned distance P cm from the mobile phone motor in the current cycle includes:
  • the total movement distance of the slave handheld motor in the current cycle is determined as the maximum planned distance P cm of the slave handheld motor in the current cycle.
  • the movement trajectory of the slave handheld motor in the current cycle is planned according to the initial movement parameters, include:
  • the slave motor According to the initial motion parameters and the second acceleration, detect whether the slave motor can decelerate from the initial speed V s to zero in the current cycle;
  • the slave motor can decelerate from the initial speed V s to zero in the current cycle, obtain the intermediate position and intermediate acceleration when the slave motor decelerates from the initial speed V s to zero;
  • the motion trajectory of the slave motor for the remaining duration of the current cycle is planned; where the initial speed of the slave motor when it is in the middle position is zero,
  • the initial acceleration is the intermediate acceleration.
  • an implementable manner of the first aspect also includes:
  • the plan is to first decelerate to the maximum acceleration of the deceleration section according to the second acceleration, and then in the remaining time of the current cycle, According to the maximum acceleration of the deceleration section, the motion will decelerate uniformly until the end of the current cycle.
  • an implementable manner of the first aspect also includes:
  • the movement trajectory of the slave motor in the current cycle is planned to decelerate to the end of the current cycle according to the second acceleration.
  • obtaining the maximum planned distance P cm from the mobile phone motor in the current cycle includes:
  • the difference between the final position and the actual position of the slave motor in the previous cycle is determined as the maximum planned distance P cm of the slave motor in the current cycle.
  • embodiments of the present application provide a control device for a master-slave robot.
  • the slave hand of the master-slave robot is provided with at least one slave motor.
  • the device is used to control the movement of the slave motor; the device includes:
  • the mapping position acquisition module is used to obtain the mapping position of the slave mobile phone in the current cycle; the mapping position is used to represent The number of mapped rotation pulses is directly mapped based on the three-dimensional coordinates of the master hand and the mechanical structure of the master-slave robot.
  • the three-dimensional coordinates of the master hand are collected according to the preset collection cycle, and the current cycle is any collection cycle;
  • the initial motion parameter acquisition module is used to obtain the initial motion parameters of the slave mobile phone in the current cycle; the initial motion parameters include the initial speed V s and the initial acceleration A s ;
  • the motion planning module is used to plan the motion trajectory of the slave mobile phone in the current cycle based on the initial motion parameters
  • the maximum planning distance acquisition module is used to obtain the maximum planning distance P cm from the mobile phone in the current cycle
  • the target distance determination module is used to determine the value that can be reached first among the mapping movement distance and the maximum planning distance P cm according to the initial movement parameters, starting from the actual position of the mobile phone in the previous cycle; mapping movement The distance is the difference between the mapped position of the slave mobile motor in the current cycle and the actual position of the slave mobile motor in the previous cycle. The actual position is used to represent the actual number of rotation pulses.
  • the previous cycle is the previous acquisition cycle of the current cycle;
  • the actual position determination module is used to determine the sum of the actual position of the slave handheld motor in the previous cycle and the target distance as the actual position of the slave handheld motor in the current cycle.
  • Embodiments of the present application provide a control method and device for a master-slave robot.
  • a virtual axis is virtually added between the master hand and the slave hand.
  • the movement trajectory of the slave hand motor in the current cycle is planned and Obtain the maximum planning distance, and finally determine the value that can be reached first among the mapping movement distance and the maximum planning distance as the actual distance that the slave mobile phone needs to move in the current cycle.
  • the master hand is directly mapped to the slave motor, which exceeds the limit of the slave motor and causes the motor to report an error.
  • excessive acceleration and speed in the movement of the master hand can be reduced. , reduce it to an acceptable level for the slave handheld motor, and extend the service life of the slave handheld motor.
  • Figure 1 is a schematic workflow diagram of a master-slave robot control method provided by an embodiment of the present application
  • FIG. 2 is a schematic flowchart of an implementation of step S103 of the master-slave robot control method provided by the embodiment of the present application;
  • FIG. 3 is a schematic flowchart of another implementation of step S103 of the master-slave robot control method provided by the embodiment of the present application;
  • Figure 4 is a schematic diagram comparing the movement of the slave motor between the control method of the master-slave robot provided by the embodiment of the present application and the control method of the traditional master-slave robot;
  • Figure 5 is a schematic structural diagram of a control device for a master-slave robot provided by an embodiment of the present application.
  • the following embodiment discloses a control method of a master-slave robot.
  • the control method provided by the embodiment of this application is applied to the master-slave robot.
  • the master-slave robot includes a master hand and at least one slave hand. Each slave hand has a one-to-one correspondence with the master hand, and each slave hand moves synchronously. Each slave hand is provided with at least one slave motor.
  • the control method provided by the embodiment of the present application is specifically used to control the movement of any slave hand motor.
  • the master hand corresponds to only one slave hand, and And the slave hand is only provided with one slave motor as an example for detailed explanation.
  • the main hand mentioned in the embodiments of this application can represent a certain set point on the main hand.
  • the set point can be located at the center of the palm or at the tip of the finger.
  • the specific point can be determined based on experience and actual conditions.
  • the embodiments of this application are for This is not specifically limited.
  • the three-dimensional coordinates of the main hand represent the spatial three-dimensional coordinates of the set point.
  • the slave hand mentioned in the embodiments of this application can represent the target setting point of the slave hand, such as the end point of the slave hand. The details can be determined based on experience and actual conditions. This is not specifically limited in the embodiments of this application. .
  • the three-dimensional coordinates of the slave hand represent the spatial three-dimensional coordinates of the target set point, and are in the same spatial coordinate system as the three-dimensional coordinates of the master hand.
  • the position of the slave hand motor represents the number of rotation pulses of the slave hand motor.
  • the movement of the slave hand motor refers to the rotation.
  • the speed of the slave hand motor refers to the rotation speed.
  • the acceleration of the slave hand motor refers to the rotation acceleration.
  • the slave hand motor refers to the rotational acceleration.
  • the movement direction of the motor includes forward and reverse rotation.
  • a control method for a master-slave robot provided by an embodiment of the present application specifically includes the following steps:
  • Step S101 Obtain the mapping position of the slave mobile phone in the current cycle.
  • the mapping position is used to represent the number of mapping rotation pulses, which is directly mapped based on the three-dimensional coordinates of the master hand and the mechanical structure of the master-slave robot.
  • the three-dimensional coordinates of the main hand are collected according to the preset collection cycle.
  • the current cycle is any collection cycle. Specifically, the collection period can be preset according to needs and actual conditions, which is not specifically limited in the embodiments of the present application.
  • the three-dimensional coordinates of the main hand refer to the spatial coordinates of the main hand position at the end of each acquisition cycle.
  • the mapping position of the slave mobile phone in the current cycle refers to the mapping position of the slave mobile phone at the end of the current cycle.
  • Step S102 Obtain the initial motion parameters of the slave mobile phone in the current cycle.
  • the initial motion parameters include initial velocity V s and initial acceleration A s .
  • the actual positions of the slave mobile phone in the three acquisition cycles before the current cycle may be first obtained respectively.
  • the first actual position of the slave mobile phone in the previous cycle refers to the actual position of the slave mobile phone at the end of the previous cycle.
  • the second actual position of the slave mobile phone in the previous collection cycle of the previous cycle refers to the actual position of the slave mobile phone at the end of the previous collection cycle in the previous cycle.
  • the third actual position of the slave handheld motor in the previous collection cycle of the previous cycle that is, the third actual position of the slave handheld motor in the third collection cycle before the current cycle, refers to the third actual position of the slave handheld motor in the third collection cycle before the current cycle.
  • the actual position at the end of the third collection period before the current period refers to the third actual position of the slave handheld motor in the third collection cycle before the current cycle.
  • the initial velocity V s and the initial acceleration A s can be determined through the following steps:
  • Step 1 Determine the first average speed of the slave mobile phone in the previous cycle based on the difference between the first actual position and the second actual position and the duration T of the acquisition cycle.
  • first average speed (first actual position - second actual position)/T.
  • Step 2 Determine the second average speed of the slave mobile phone in the previous collection cycle of the previous cycle based on the difference between the second actual position and the third actual position and the duration T of the collection cycle.
  • the second average speed (second actual position - third actual position)/T.
  • Step 3 Determine the average acceleration based on the second average speed, the first average speed, and the duration T of the acquisition period.
  • average acceleration (first average speed - second average speed)/T.
  • Step 4 Determine the first average speed as the initial speed V s and the average acceleration as the initial acceleration A s .
  • the average value of the first average speed and the second average speed can be Determine the initial speed V s .
  • set the initial speed V s first average speed+average acceleration ⁇ duration of the current period/2.
  • the initial velocity V s and initial acceleration A s can also be obtained through jerk. In this way, more application scenarios can be matched according to actual conditions, resulting in greater flexibility.
  • the initial motion parameters are known, there is no need to calculate the initial motion parameters based on the data of the first three periods of the current period.
  • the data delay of three cycles can be reduced, and the data provided by the embodiments of the present application can be applied at any time (for example, when the master hand is required to move past a certain position, or when the master-slave hand linkage is required after moving for a specific time, etc.) method, with wider adaptability.
  • the control method provided by the embodiment of the present application may also include: first, based on the mapping position P 4 of the slave mobile phone in the current cycle and the actual position of the slave mobile phone in the previous cycle.
  • Position P 3 determines the first movement direction of the slave handheld motor in the current cycle. For example, if the mapped position P 4 of the mobile phone in the current cycle is 10,000 pulses and the actual position P 3 in the previous cycle is 5,000 pulses, then the first movement direction is determined to be forward rotation. From the mapped position P 4 of the handheld motor in the current cycle to 5,000 pulses and the actual position P 3 in the previous cycle to 10,000 pulses, the first movement direction is determined to be reverse.
  • the second movement direction of the slave hand motor in the previous cycle is determined. Finally, it is detected whether the first movement direction and the second movement direction are the same. Among them, the actual position is used to represent the actual number of rotation pulses.
  • the previous cycle is the acquisition cycle before the current cycle.
  • Step S103 Plan the movement trajectory of the slave handheld motor in the current cycle based on the initial movement parameters.
  • FIG. 2 is a schematic flowchart of an implementation of step S103 of the master-slave robot control method provided by the embodiment of the present application.
  • step S103 can be planned based on the S-shaped speed curve through the following implementation:
  • Step S201 detect whether the initial speed V s is equal to the limit speed V m of the slave hand motor. If the initial speed V s is equal to the limit speed V m , step S202 is performed. If the initial speed V s is less than the limit speed V m , step S203 is executed.
  • Step S202 For the motion trajectory of the slave mobile phone in the current period, plan to move at a constant speed according to the limit speed V m until the end of the current period.
  • Step S203 detect whether the initial acceleration As is equal to the limit acceleration Am . If the initial acceleration As is equal to the limit acceleration Am , step S204 is performed. If the initial acceleration As is less than the limit acceleration Am , step S208 is performed.
  • Step S204 determine the time required to uniformly accelerate from the initial speed V s to the limit speed V m according to the limit acceleration Am . First duration.
  • Step S205 detect whether the first duration is less than the duration of the current cycle. If the first duration is less than the duration of the current period, step S206 is performed. If the first duration is greater than the duration of the current period, step S207 is executed.
  • the plan is to uniformly accelerate to the limit speed V m according to the limit acceleration A m .
  • the case where the first duration is equal to the duration of the current period will not be explained separately.
  • Step S206 for the motion trajectory of the slave handheld motor in the current period, it is planned to first accelerate uniformly according to the limit acceleration Am to the limit speed Vm , and then move uniformly according to the limit speed Vm to the end of the current period in the remaining time of the current period.
  • the slave motor can reach the limit speed V m in the current cycle, so the plan is to first move at a uniform acceleration and then continue to move at a uniform speed.
  • Step S207 For the movement trajectory of the slave handheld motor in the current period, it is planned to uniformly accelerate from the initial speed Vs to the end of the current period according to the limit acceleration Am .
  • the slave motor cannot reach the limit speed V m in the current cycle, so the plan is to accelerate the entire cycle uniformly according to the maximum capability.
  • Step S208 Determine the second time period required to accelerate from the initial acceleration As to the limit acceleration Am according to the first jerk.
  • J 1 is the first jerk, which represents the preset jerk in the uniform acceleration section, and the value of J 1 is a positive value.
  • Step S209 detect whether the second duration is less than or equal to the duration of the current period. If the second duration is less than or equal to the duration of the current period, step S210 is performed. If the second duration is greater than the duration of the current period, step S217 is executed.
  • Step S210 Obtain the intermediate speed after the second duration.
  • the intermediate speed V after the end of the second duration can be determined by the following formula:
  • T a is the second duration
  • J 1 is the first acceleration
  • V s is the initial speed.
  • Step S211 detect whether the intermediate speed is less than the limit speed V m . If the intermediate speed is less than the limit speed V m , step S212 is executed. If the intermediate speed is greater than the limit speed V m , step S216 is executed.
  • the plan is to accelerate uniformly from the initial speed V s to the limit speed V m according to the first jerk. At this time, the slave motor reaches both its maximum speed and maximum acceleration.
  • Step S212 determine the third time period required to uniformly accelerate from the intermediate speed to the limit speed V m according to the limit acceleration A m .
  • V is the intermediate speed.
  • Step S213, detect whether the sum of the second duration and the third duration is less than the duration of the current period. If it is less than, step 214 is executed. If it is greater, perform step 215.
  • the first acceleration Accelerate to the limit acceleration Am and then accelerate uniformly to the limit speed V m according to the limit acceleration Am .
  • Step S214 for the motion trajectory of the slave mobile phone in the current cycle, it is planned to first accelerate to the limit acceleration Am according to the first jerk, then uniformly accelerate to the limit speed V m according to the limit acceleration Am , and finally in the remaining time of the current cycle , moving at a constant speed according to the limit speed V m to the end of the current period.
  • the slave motor can reach both the limit speed V m and the limit acceleration A m in the current cycle.
  • Step S215 for the movement trajectory of the slave mobile phone in the current period, it is planned to first accelerate from the initial acceleration As to the limit acceleration Am , and then accelerate uniformly according to the limit acceleration Am in the remaining time of the current period until the end of the current period.
  • the slave motor can reach the limit acceleration A m in the current cycle, but cannot reach the limit speed V m .
  • Step S216 for the motion trajectory of the slave handheld motor in the current cycle, it is planned to first accelerate uniformly from the initial speed V s to the limit speed V m according to the first jerk, and then uniformly accelerate according to the limit speed V m in the remaining time of the current period. Movement to the end of the current period.
  • the time T v required to uniformly accelerate from the initial speed V s to the limit speed V m can be determined by the following formula:
  • J 1 is the first jerk.
  • the slave motor cannot reach the limit acceleration A m in the current cycle, but it can reach the limit speed V m .
  • Step S217 Determine the final velocity from the initial velocity V s according to the first jerk to the final velocity after the end of the current period.
  • J 1 is the first jerk
  • T is the length of the current cycle.
  • Step S218, detect whether the final speed is less than or equal to the limit speed V m . If the final speed is less than or equal to the limit speed V m , step S219 is executed. If the final speed is greater than the limit speed V m , step S216 is executed.
  • Step S219 For the movement trajectory of the slave handheld motor in the current cycle, plan the movement from the initial speed V s to the end of the current cycle according to the first jerk.
  • the slave motor can neither reach the limit speed V m nor the limit acceleration Am in the current cycle.
  • the speed of the slave hand motor can be relatively gentle, and no error will be reported. Both the speed and acceleration can meet the parameter requirements of the system motor, and the motion trajectory can roughly follow the movement of the master hand. trajectory.
  • FIG. 3 is a schematic flowchart of another implementation of step S103 of the master-slave robot control method provided by the embodiment of the present application.
  • step S103 can be implemented in the following implementation manner:
  • Step S301 based on the initial motion parameters and the second acceleration, detect whether the slave handheld motor can decelerate from the initial speed V s to zero in the current cycle. If it is possible to decelerate from the initial speed V s to zero, step S302 is executed. If it is impossible to decelerate from the initial speed V s to zero, step S304 is executed.
  • the second jerk represents the preset jerk of the uniform acceleration and deceleration section, and the value is a negative value.
  • Step S302 Obtain the intermediate position and intermediate acceleration when the slave handheld motor decelerates from the initial speed V s to zero.
  • the intermediate position and intermediate acceleration can be solved after kinematic analysis.
  • Step S303 Plan the motion trajectory of the slave handheld motor for the remaining duration of the current cycle based on the motion parameters of the slave handheld motor when it is at the intermediate position and the remaining duration of the current cycle. Among them, the initial speed of the slave mobile phone when it is in the middle position is zero, and the initial acceleration is the middle acceleration.
  • the motion trajectory planning method may refer to the aforementioned steps S201 to S219, which will not be described again here.
  • Step S304 detect whether the slave motor can reach the maximum acceleration of the deceleration section in the current cycle. If yes, execute step S305; otherwise, execute step S306.
  • the slave handheld motor it is possible to first determine the time required for the slave handheld motor to decelerate from the initial motion parameters to the maximum acceleration of the deceleration section according to the second acceleration. If the time is greater than the length of the current cycle, it is determined that the slave handheld motor cannot reach the maximum acceleration in the current cycle. The maximum acceleration of the deceleration section, otherwise, determine that the slave motor can reach the maximum acceleration of the deceleration section in the current cycle.
  • Step S305 for the motion trajectory of the slave handheld motor in the current cycle, it is planned to first decelerate according to the second acceleration to the maximum acceleration of the deceleration section, and then in the remaining time of the current cycle, according to the maximum acceleration of the deceleration section, uniformly decelerate to the end of the current cycle.
  • J 2 is the second jerk, which represents the preset jerk in the uniform acceleration and deceleration section, and the value is a negative value.
  • a smd is the maximum acceleration of the deceleration section.
  • the intermediate speed V 2 after the end of the fourth duration T 4 can be determined by the following formula:
  • T 4 is the fourth duration
  • J 2 is the second acceleration
  • V s is the initial velocity
  • Step S306 For the movement trajectory of the slave mobile phone in the current period, plan to decelerate the movement according to the second jerk until the end of the current period.
  • the speed of the slave hand motor can be relatively gentle, and no error will be reported. Both the speed and acceleration can meet the parameter requirements of the system motor, and the motion trajectory can roughly follow the movement of the master hand. trajectory.
  • trajectory planning can also be carried out in other ways, such as planning based on a T-shaped speed curve. In this way, trajectory planning can be more flexible and thus have a wider range of applications.
  • step S103 can be executed to plan the motion trajectory.
  • the current cycle is the first cycle after the initial acquisition cycle, there is no need to detect the movement direction. The default is that the movement direction is the same as that of the previous cycle, and the initial velocity and initial acceleration are both zero. If the current cycle is the cycle after the first cycle after the initial acquisition cycle, the motion direction needs to be detected first.
  • Step S104 Obtain the maximum planned distance P cm of the slave mobile phone in the current period.
  • step S104 corresponds to the aforementioned various planning methods.
  • the slave mobile phone when the first movement direction and the second movement direction are the same, according to the above steps S201 to S219, after planning the movement trajectory of the slave mobile phone in the current period, the slave mobile phone can be The total distance of movement in the current cycle is determined as the maximum planned distance P cm of the slave mobile phone in the current cycle.
  • V m is the limit speed
  • T is the length of the current cycle.
  • the maximum planning distance P cm can be determined by the following formula:
  • V s is the initial speed
  • T 1 is the first duration
  • a m is the limit acceleration
  • V m is the limit speed
  • T is the duration of the current period.
  • V s is the initial velocity
  • a m is the limit acceleration
  • T is the length of the current cycle.
  • the maximum planning distance P cm can be determined by the following formula:
  • J 1 is the first jerk
  • T a is the second duration
  • T 3 is the third duration
  • T is the duration of the current cycle
  • V is the intermediate speed after the end of the second duration
  • V s is the initial speed
  • V m is the ultimate speed.
  • the maximum planning distance P cm can be determined by the following formula:
  • J 1 is the first jerk
  • T a is the second duration
  • T is the duration of the current cycle
  • V is the intermediate speed after the end of the second duration
  • V s is the initial speed
  • a m is the limit acceleration.
  • the maximum planning distance P cm can be determined by the following formula:
  • J 1 is the first jerk
  • V s is the initial speed
  • V m is the limit speed
  • T is the length of the current cycle
  • T v is the time required to uniformly accelerate from the initial speed V s to the limit speed V m .
  • J 1 is the first jerk
  • V s is the initial speed
  • T is the length of the current cycle.
  • the slave hand motor when the first movement direction is opposite to the second movement direction, according to the above steps S301 to S306, after planning the movement trajectory of the slave hand motor in the current cycle, the slave hand motor can first be obtained.
  • the final position of the motor at the end of the current cycle, and then the difference between the final position and the actual position of the slave motor in the previous cycle is determined as the maximum planned distance P cm of the slave motor in the current cycle.
  • the final position is equal to the total distance of movement from the initial position of the hand motor (that is, the actual position in the previous cycle) plus the current cycle. It should be noted that since reverse motion is involved, the motion distance is a directional vector.
  • the slave mobile phone After executing step S303, the slave mobile phone first maintains the deceleration movement in the same direction as the initial speed until the speed is reduced to zero, and then moves in the reverse direction until the end of the current cycle.
  • the final position is the result of adding the total movement distance of the deceleration section to the initial position of the handheld motor, and then subtracting the total movement distance of the reverse movement.
  • the planning of the aforementioned steps S201 to S219 can be referred to.
  • the total distance of the reverse motion can be calculated with reference to the corresponding maximum planning distance calculation method in an implementation of step S104.
  • the duration of the reverse movement is the remaining duration of the current cycle, and the initial speed of the reverse movement is zero.
  • the slave handheld motor After executing steps S305 and S306, the slave handheld motor maintains the deceleration movement in the same direction as the initial speed until the end of the current cycle. At this time, the final position is the result obtained from the initial position of the mobile phone plus the maximum planning distance P cm .
  • the maximum planning distance P cm can be determined by the following formula:
  • J 2 is the second jerk
  • V s is the initial speed
  • T is the duration of the current cycle
  • T 4 is the fourth duration
  • V 2 is the intermediate speed after the end of the fourth duration T 4
  • a smd is the maximum deceleration section acceleration.
  • J 2 is the first jerk
  • V s is the initial speed
  • T is the length of the current cycle.
  • Step S105 Starting from the actual position of the mobile phone motor in the previous cycle, according to the initial motion parameters, the value that can be reached first among the mapping motion distance and the maximum planned distance P cm is determined as the target distance.
  • the mapping movement distance is the difference between the mapped position of the slave mobile phone in the current cycle and the actual position of the slave mobile phone in the previous cycle.
  • the actual position is used to represent the actual number of rotational pulses.
  • the previous cycle is the acquisition cycle before the current cycle.
  • the actual position of the slave handheld motor in the previous cycle is 5000 pulses
  • the mapped position of the slave handheld motor in the current cycle is 2000 pulses
  • the mapped movement distance is -3000 pulses.
  • the slave handheld motor After planning the motion trajectory according to the initial motion parameters of the slave handheld motor, the slave handheld motor first continues to decelerate in the same direction as the initial speed until the speed is reduced to zero.
  • the intermediate position is 6500 pulses, and then moves in the reverse direction to the current
  • the final position is 3000 pulses
  • the maximum planning distance P cm is -2000 pulses.
  • the slave motor needs to move in the reverse direction, it is necessary to reduce the speed to zero before it can move in the reverse direction.
  • Target distance is -2000 pulses.
  • the actual position of the slave handheld motor in the previous cycle is 5000 pulses
  • the mapped position of the slave handheld motor in the current cycle is 2000 pulses
  • the mapped movement distance is -3000 pulses.
  • the movement trajectory planned from the handheld motor is to decelerate in the same direction as the initial speed until the end of the current cycle. At this time, the speed has not reduced to zero, and the final position is 8000 pulses.
  • the maximum planned distance P cm is 3000 pulses. According to the movement of the motor, the value that can be reached first is 8000 pulses. Therefore, the target distance is determined to be 3000 pulses. It should be noted that in the embodiment of the present application, the symbols of the mapping movement distance and the maximum planning distance P cm are both used to represent the direction.
  • Step S106 The sum of the actual position of the slave handheld motor in the previous cycle and the distance to the target is determined as the actual position of the slave handheld motor in the current cycle.
  • the control method provided by the embodiment of the present application is equivalent to adding a virtual axis between the master hand and the slave hand. After processing the data through the virtual axis, the value that can be reached first among the movement distance and the maximum planning distance will be mapped. Send it to the slave motor. In this way, you can avoid the problem of the master hand being directly mapped to the slave motor, causing the motor to report an error due to exceeding the limit of the slave motor, so that the master hand can be moved during the motion while ensuring that the overall trajectory remains roughly unchanged. Large acceleration and speed are reduced to an acceptable level for the slave hand motor, thereby extending the service life of the slave hand motor.
  • control method provided by the embodiment of the present application may also include:
  • Step S107 Determine the actual three-dimensional coordinates of the slave hand in the current cycle based on the actual position of the slave hand motor in the current cycle.
  • Step S108 detect whether the actual three-dimensional coordinates exceed the limit coordinates of the slave hand. If exceeded, step S109 is executed. If it does not exceed, step S110 is executed.
  • Step S109 Obtain the corrected position of the slave hand motor in the current cycle based on the limit coordinates of the slave hand.
  • Step S110 detect whether the multi-axis motion of each slave hand of the master-slave robot in the current cycle is synchronized. If not synchronized, execute step S111. If synchronized, execute step S116.
  • Step S111 According to the target three-dimensional coordinates of each slave hand in the current cycle and the mechanical structure of the master-slave robot, the target position of the slave motor of each slave hand is obtained using the inverse kinematics solution method.
  • Step S112 Obtain the target three-dimensional coordinates of the master hand based on the target position of the slave motor on each slave hand.
  • Step S113 detect whether the target three-dimensional coordinates of the main hand exceed the limit coordinates of the main hand. If the target three-dimensional coordinates of the main hand exceed the limit coordinates of the main hand, step S114 is executed. If the target three-dimensional coordinates of the main hand do not exceed the limit coordinates of the main hand, step S115 is executed.
  • Step S114 correct the three-dimensional coordinates of the main hand in the current period to the limit coordinates of the main hand. Then return to step S101 to reacquire the mapping position of each slave mobile phone in the current cycle.
  • Step S115 correct the three-dimensional coordinates of the main hand in the current period to the target three-dimensional coordinates of the main hand. Then return to step S101 to reacquire the mapping position of each slave mobile phone in the current cycle.
  • Step S116 detect whether the actual position of the slave handheld motor in the current cycle has been corrected. If it has not been corrected, step S117 is executed. If corrected, step S118 is executed.
  • Step S117 Control the movement of the handheld motor to the actual position of the current cycle.
  • Step S118 Control the movement of the hand motor to the correction position of the current cycle.
  • the motion restriction parameters of the master hand can be obtained through the forward and inverse solutions of the robot motion, and the motion restriction parameters of the master hand can be re-executed based on the motion restriction parameters of the master hand.
  • the embodiments of the present application provide The virtual axis algorithm is processed and finally mapped to the joints of the slave hand, which can effectively avoid the multi-axis coordination problem that occurs in the slave hand linkage under special circumstances.
  • Figure 4 is a schematic diagram comparing the movement of the slave motor between the control method of the master-slave robot provided by the embodiment of the present application and the traditional control method of the master-slave robot.
  • the horizontal axis of each curve is the sampling period point.
  • curve A1 is the movement distance change curve of the slave hand motor obtained by using the traditional master-slave robot control method (that is, direct mapping from the master hand to the slave hand motor);
  • curve B1 is the change curve of the slave hand motor using this method.
  • Curve A1 is a motion trajectory mapped directly from the motion of the master hand.
  • the slave hand motor in curve B1 roughly follows the movement trajectory of the master hand, and the slope (ie, movement speed) is lower than the original data of curve A1.
  • the slave hand motor in curve B1 roughly follows the movement trajectory of the master hand, and the slope (ie, movement speed) is lower than the original data of curve A1.
  • curve A2 is the movement speed change curve of the slave hand motor obtained by using the traditional master-slave hand robot control method
  • curve B2 is obtained by using the master-slave hand robot control method provided by the embodiment of the present application.
  • the movement speed change curve of the slave hand motor is the movement speed change curve of the slave hand motor.
  • the speed in curve A2 is discrete, with obvious sudden changes, and the speed is often very large, while the speed in curve B2 is relatively gentle. When it reaches the limit value of the slave motor, it no longer rises and keeps moving at a constant speed.
  • curve A3 is the traditional master-slave mobile phone
  • the acceleration change curve of the slave hand motor obtained by the control method of the robot
  • Curve B3 is the acceleration change curve of the slave hand motor obtained by the control method of the master-slave hand robot provided by the embodiment of the present application.
  • the acceleration in curve A3 is very large and cannot usually be achieved by a handheld motor.
  • the acceleration change in curve B3 is very small, and there are only some sudden changes when changing direction. Compared with the data in curve A3, the motor will run more smoothly and will not report an error.
  • embodiments of the present application provide a control device for a master-slave robot.
  • the slave hand of the master-slave robot is provided with at least one slave motor, and the control device is used to control the movement of the slave motor.
  • the control device of the master-slave robot provided by the embodiment of the present application includes: mapping position acquisition module 501, initial motion parameter acquisition module 502, motion planning module 503, maximum planning distance acquisition module 504, target distance determination module 505 and actual position determination module 506.
  • the device may also include more or fewer units and modules such as a slave hand position detection module and a slave hand motion control module. This embodiment does not limit the structure of the device.
  • the mapping position acquisition module 501 is used to obtain the mapping position of the slave hand motor in the current cycle; the mapping position is used to represent the number of mapping rotation pulses and is directly mapped based on the three-dimensional coordinates of the master hand and the mechanical structure of the master-slave hand robot.
  • the three-dimensional coordinates of the hand are collected according to the preset collection cycle, and the current cycle is any collection cycle;
  • the initial motion parameter acquisition module 502 is used to acquire the initial motion parameters of the slave mobile phone in the current cycle; the initial motion parameters include the initial speed V s and the initial acceleration As ;
  • the motion planning module 503 is used to plan the motion trajectory of the slave handheld motor in the current cycle based on the initial motion parameters
  • the maximum planned distance acquisition module 504 is used to obtain the maximum planned distance P cm from the mobile phone in the current cycle
  • the target distance determination module 505 is used to determine the value that can be reached first among the mapping movement distance and the maximum planning distance P cm according to the initial movement parameters, starting from the actual position of the mobile phone in the previous cycle, as the target distance; mapping The movement distance is the difference between the mapped position of the slave mobile motor in the current cycle and the actual position of the slave mobile motor in the previous cycle. The actual position is used to represent the actual number of rotation pulses.
  • the previous cycle is the previous acquisition cycle of the current cycle;
  • the actual position determination module 506 is used to determine the sum of the actual position of the slave handheld motor in the previous cycle and the distance to the target as the actual position of the slave handheld motor in the current cycle.
  • the device further includes:
  • the slave hand position detection module is used to determine the actual three-dimensional coordinates of the slave hand in the current cycle based on the actual position of the slave hand motor in the current cycle; and detect whether the actual three-dimensional coordinates exceed the limit coordinates of the slave hand;
  • the slave hand motion control module is used to control the movement of the slave hand motor to the actual position of the current cycle if the actual three-dimensional coordinates do not exceed the limit coordinates of the slave hand.
  • the slave hand motion control module is also used to: if the actual three-dimensional coordinates exceed the limit coordinates of the slave hand, obtain the corrected position of the slave hand motor in the current cycle based on the limit coordinates of the slave hand; and, control Corrected position from hand motor movement to current cycle.
  • the slave hand motion control module is also used to:
  • the method of inverse kinematics solution is used to obtain each slave hand.
  • the target position of the slave motor on the slave hand according to the target position of the slave motor on each slave hand, the target three-dimensional coordinates of the master hand are obtained; if the target three-dimensional coordinates of the master hand exceed the limit coordinates of the master hand, the master hand will be in the current cycle
  • the three-dimensional coordinates are corrected to the limit coordinates of the master hand; based on the limit coordinates of the master hand, the mapping position of the slave motor in the current cycle is reacquired until the actual position of the slave motor in the current cycle is re-determined.
  • the slave motion control module is also used to:
  • the three-dimensional coordinates of the main hand in the current period are corrected to the target three-dimensional coordinates of the main hand and sent back to the mapping position acquisition module 501.
  • the initial motion parameter acquisition module 502 is specifically used to:
  • the initial motion parameter acquisition module 502 is specifically used to:
  • the first actual position is the actual position of the slave handheld motor in the previous cycle.
  • the second actual position is the actual position of the slave mobile phone in the previous collection cycle of the previous cycle; according to the difference between the second actual position and the third actual position, and the length of the collection cycle, determine the position of the slave mobile phone in the previous cycle.
  • the second average speed in the previous collection cycle; the third actual position is the actual position of the slave mobile phone in the third collection cycle before the current cycle; based on the second average speed, the first average speed, and the length of the collection cycle , determine the average acceleration; determine the first average speed as the initial speed V s , and determine the average acceleration as the initial acceleration A s .
  • the device further includes:
  • the direction detection module is used to determine the first movement direction of the slave motor in the current cycle based on the mapped position of the slave motor in the current cycle and the actual position of the slave motor in the previous cycle; The actual position of the cycle, and the actual position of the slave handheld motor in the previous collection cycle of the previous cycle, determine the second movement direction of the slave handheld motor in the previous cycle; detect whether the first movement direction and the second movement direction are the same.
  • the motion planning module 503 is specifically used to:
  • the motion planning module 503 is also specifically used to:
  • the initial speed V s is less than the limit speed V m , then check whether the initial acceleration A s is equal to the limit acceleration A m ; if the initial acceleration A s is equal to the limit acceleration A m , then determine whether the initial speed V s will accelerate uniformly according to the limit acceleration A m
  • the first duration required to reach the limit speed V m if the first duration is less than the length of the current cycle, then for the movement trajectory of the slave mobile phone in the current cycle, the plan is to first uniformly accelerate to the limit speed V m according to the limit acceleration A m , and then In the remaining time of the current period, it moves at a constant speed according to the limit speed V m until the end of the current period.
  • the motion planning module 503 is also specifically used to:
  • the plan is to uniformly accelerate the movement from the initial speed V s to the end of the current period according to the limit acceleration Am .
  • the motion planning module 503 is also specifically used to:
  • the plan is to first accelerate to the limit acceleration A m according to the first jerk, then uniformly accelerate to the limit speed V m according to the limit acceleration A m , and finally in the current cycle During the remaining time, it moves at a constant speed according to the limit speed V m until the end of the current period.
  • the motion planning module 503 is also specifically used to:
  • the sum of the second duration and the third duration is greater than the duration of the current period, then for the motion trajectory of the slave mobile phone in the current period, it is planned to first accelerate from the initial acceleration A s to the limit acceleration A m , and then in the remaining time of the current period , uniformly accelerating according to the limit acceleration A m to the end of the current period.
  • the motion planning module 503 is also specifically used to:
  • the plan is to accelerate uniformly from the initial speed V s to the limit speed V m according to the first jerk, and then accelerate it uniformly from the initial speed V s to the limit speed V m during the remaining time of the current cycle. , moving at a constant speed according to the limit speed V m to the end of the current period.
  • the motion planning module 503 is also specifically used to:
  • the initial speed V s determines the initial speed V s , according to the first jerk, uniform acceleration to the final speed after the end of the current cycle; if the final speed is less than the limit speed V m , then for the slave motor
  • the motion trajectory in the current cycle is planned to accelerate from the initial speed V s according to the first jerk to the end of the current cycle.
  • the motion planning module 503 is also specifically used to:
  • the plan is to first accelerate uniformly from the initial speed V s to the limit speed V m according to the first jerk, and then continue the rest of the current cycle. Within, it moves at a constant speed according to the limit speed V m until the end of the current period.
  • the maximum planning distance acquisition module 504 is specifically used to:
  • the total movement distance of the slave handheld motor in the current cycle is determined as the maximum planned distance P cm of the slave handheld motor in the current cycle.
  • the motion planning module 503 is specifically used to:
  • the slave motor According to the initial motion parameters and the second acceleration, it is detected whether the slave motor can decelerate from the initial speed V s to zero in the current cycle; if the slave motor can decelerate from the initial speed V s to zero in the current cycle, then Obtain the intermediate position and intermediate acceleration of the slave motor when it decelerates from the initial speed V s to zero; based on the motion parameters of the slave motor at the intermediate position and the remaining duration of the current cycle, calculate the remaining duration of the slave motor in the current cycle.
  • the motion trajectory is planned; among them, the initial speed of the slave mobile phone when it is in the middle position is zero, and the initial acceleration is the middle acceleration.
  • the motion planning module 503 is also specifically used to:
  • the slave motor cannot decelerate from the initial speed V s to zero in the current cycle, check whether the slave motor can reach the maximum acceleration of the deceleration section in the current cycle; if the slave motor can reach the maximum acceleration of the deceleration section in the current cycle, For the motion trajectory of the slave handheld motor in the current cycle, the plan is to first decelerate to the maximum acceleration of the deceleration section according to the second acceleration, and then decelerate uniformly to the end of the current cycle according to the maximum acceleration of the deceleration section during the remaining time of the current cycle.
  • the motion planning module 503 is also specifically used to:
  • the movement trajectory of the slave motor in the current cycle is planned to decelerate to the end of the current cycle according to the second acceleration.
  • the maximum planning distance acquisition module 504 is specifically used to:
  • the control device provided by the embodiment of the present application is equivalent to adding a virtual axis between the master hand and the slave hand. After processing the data through the virtual axis, the value that can be reached first among the movement distance and the maximum planning distance will be mapped. Send it to the slave motor. In this way, you can avoid the problem of the master hand being directly mapped to the slave motor, causing the motor to report an error due to exceeding the limit of the slave motor, so that the master hand can be moved during the motion while ensuring that the overall trajectory remains roughly unchanged. Large acceleration and speed are reduced to an acceptable level for the slave hand motor, thereby extending the service life of the slave hand motor.

Landscapes

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

Abstract

本申请实施例提供一种主从手机器人的控制方法及装置。所述方法包括：获取从手电机在当前周期的映射位置和初始运动参数后，对从手电机在当前周期的运动轨迹进行规划并获取最大规划距离，最终将映射运动距离和最大规划距离中能够优先达到的值确定为从手电机在当前周期需要运动的实际距离。如此，可以避免主手直接映射到从手电机所产生的超出从手电机极限导致电机报错的问题，从而在保证总体轨迹大致不变的情况下，可以将主手运动中过大的加速度、速度，减小至从手电机可接受的程度，延长从手电机的使用寿命。

Description

一种主从手机器人的控制方法及装置 技术领域
本申请涉及机器人技术领域，特别涉及一种主从手机器人的控制方法及装置。
背景技术
人手在进行微小尺度操作时往往会存在一定的局限性，例如会发生手部震颤等细微手部动作。为了克服人手在微小尺度操作的局限性，能够代替人手部分工作的主从手机器人应运而生。
主从手机器人主要包括主手和与主手对应的从手，其中，主手完全由操作者进行控制，并如实反映操作者人手的运动参数，从手通常为机械臂，机械臂上设置有至少一个关节，每个关节上设置有电机。在实际控制中，从手的各个电机的运动参数是根据主手(通常是指主手上的设定点)的三维坐标映射得到的，主手的运动通过主从手机器人的机械结构映射传递给从手，最终使得从手的目标设定点(比如末端点)跟随主手进行对应的运动。
采用上述控制方式，从手各个电机的运动参数是直接根据主手的运动映射得到的，然而，由于人手的加速度通常比电机快很多，部分情况下产生的加速度，比如：在向第一方向快速移动时突然反向加速移动，在映射到从手电机后可能会超出从手电机的极限转动加速度，进而导致电机报错，造成从手无法准确跟随主手进行运动，或者从手的电机寿命受影响的问题。
发明内容
现有的控制方法可能出现映射后的从手电机转动加速度超出极限加速度，进而导致电机报错，造成从手无法准确跟随主手进行运动，或者从手的电机寿命受影响的技术问题。为了解决这一问题，本申请实施例提供一种主从手机器人的控制方法及装置，具体地，本申请公开了以下技术方案：
第一方面，本申请实施例提供一种主从手机器人的控制方法，主从手机器人的从手上设置有至少一个从手电机，方法用于对从手电机的运动进行控制；方法包括：
获取从手电机在当前周期的映射位置；映射位置用于表示映射旋转脉冲数，是根据主手的三维坐标以及主从手机器人的机械结构直接映射得到的，主手的三维坐标是按照预设采集周期采集得到的，当前周期为任一采集周期；
获取从手电机在当前周期的初始运动参数；初始运动参数包括初始速度Vs和初始加速度As；
根据初始运动参数，对从手电机在当前周期的运动轨迹进行规划；
获取从手电机在当前周期内的最大规划距离Pcm；
从从手电机在前一周期的实际位置开始，根据初始运动参数，将映射运动距离和最大规划距离Pcm中，能够优先达到的值，确定为目标距离；映射运动距离为从手电机在当前周期的映射位置与从手电机在前一周期的实际位置的差值，实际位置用于表示实际旋转脉冲数，前一周期为当前周期的前一个采集周期；
将从手电机在前一周期的实际位置与目标距离的和，确定为从手电机在当前周期的实际位置。
结合第一方面，在第一方面的一种可实现方式中，还包括：
根据从手电机在当前周期的实际位置，确定从手在当前周期的实际三维坐标；
检测实际三维坐标是否超过从手的极限坐标；
如果实际三维坐标未超过从手的极限坐标，则控制从手电机运动到当前周期的实际位置。
结合第一方面，在第一方面的一种可实现方式中，还包括：
如果实际三维坐标超过从手的极限坐标，则根据从手的极限坐标，获取从手电机在当前周期的修正位置；
控制从手电机运动到当前周期的修正位置。
结合第一方面，在第一方面的一种可实现方式中，在控制从手电机运动之前，还包括：
如果主从手机器人的各个从手在当前周期的多轴运动不同步，则根据各个从手在当前周期的目标三维坐标，以及主从手机器人的机械结构，利用运动学逆解的方法获取各个从手上的从手电机的目标位置；
根据各个从手上的从手电机的目标位置，获取主手的目标三维坐标；
如果主手的目标三维坐标超出主手的极限坐标，则将主手在当前周期的三维坐标修正为主手的极限坐标；
根据主手的极限坐标，重新获取从手电机在当前周期的映射位置，直至重新确定出从手电机在当前周期的实际位置。
结合第一方面，在第一方面的一种可实现方式中，还包括：
如果主手的目标三维坐标未超出主手的极限坐标，则将主手在当前周期的三维坐标修正为主手的目标三维坐标；
根据主手的目标三维坐标，重新获取从手电机在当前周期的映射位置，直至重新确定出从手电机在当前周期的实际位置。
结合第一方面，在第一方面的一种可实现方式中，获取从手电机在当前周期的初始运动参数，包括：
分别获取从手电机在当前周期以前的三个采集周期的实际位置；
根据从手电机在当前周期以前的三个采集周期的实际位置，确定初始速度Vs和初始加速度As。
结合第一方面，在第一方面的一种可实现方式中，根据从手电机在当前周期以前的三个采集周期的实际位置，确定初始速度Vs和初始加速度As，包括：
根据第一实际位置与第二实际位置的差值，以及采集周期的时长，确定从手电机在前一周期内的第一平均速度；第一实际位置为从手电机在前一周期的实际位置，第二实际位置为从手电机在前一周期的前一个采集周期的实际位置；
根据第二实际位置与第三实际位置的差值，以及采集周期的时长，确定从手电机在前一周期的前一个采集周期内的第二平均速度；第三实际位置为从手电机在当前周期以前的第三个采集周期的实际位置；
根据第二平均速度、第一平均速度，以及采集周期的时长，确定平均加速度；
将第一平均速度确定为初始速度Vs，将平均加速度确定为初始加速度As。
结合第一方面，在第一方面的一种可实现方式中，在根据初始运动参数，对从手电机在当前周期的运动轨迹进行规划之前，方法还包括：
根据从手电机在当前周期的映射位置，以及从手电机在前一周期的实际位置，确定从手电机在当前周期内的第一运动方向；
根据从手电机在前一周期的实际位置，以及从手电机在前一周期的前一个采集周期的实际位置，确定从手电机在前一周期内的第二运动方向；
检测第一运动方向与第二运动方向是否相同。
结合第一方面，在第一方面的一种可实现方式中，在第一运动方向与第二运动方向相同的情况下，根据初始运动参数，对从手电机在当前周期的运动轨迹进行规划，包括：
检测初始速度Vs是否等于从手电机的极限速度Vm；
如果初始速度Vs等于极限速度Vm，则对于从手电机在当前周期的运动轨迹，规划按照极限速度Vm匀速运动至当前周期结束。
结合第一方面，在第一方面的一种可实现方式中，还包括：
如果初始速度Vs小于极限速度Vm，则检测初始加速度As是否等于极限加速度Am；
如果初始加速度As等于极限加速度Am，则确定从初始速度Vs，按照极限加速度Am匀加速到极限速度Vm所需的第一时长；
如果第一时长小于当前周期的时长，则对于从手电机在当前周期的运动轨迹，规划先按照极限加速度Am匀加速至极限速度Vm，再在当前周期的剩余时间内，按照极限速度Vm匀速运动至当前周期结束。
结合第一方面，在第一方面的一种可实现方式中，还包括：
如果第一时长大于当前周期的时长，则对于从手电机在当前周期的运动轨迹，规划按照极限加速度Am，从初始速度Vs匀加速运动至当前周期结束。
结合第一方面，在第一方面的一种可实现方式中，还包括：
如果初始加速度As小于极限加速度Am，则确定从初始加速度As，按照第一加加速度，加速到极限加速度Am所需的第二时长；
如果第二时长小于当前周期的时长，则获取第二时长结束后的中间速度；
如果中间速度小于极限速度Vm，则确定从中间速度，按照极限加速度Am匀加速到极限速度Vm所需的第三时长；
如果第二时长和第三时长的和小于当前周期的时长，则对于从手电机在当前周期的运动轨迹，规划先按照第一加加速度加速至极限加速度Am，再按照极限加速度Am匀加速至极限速度Vm，最后在当前周期的剩余时间内，按照极限速度Vm匀速运动至当前周期结束。
结合第一方面，在第一方面的一种可实现方式中，还包括：
如果第二时长和第三时长的和大于当前周期的时长，则对于从手电机在当前周期的运动轨迹，规划从初始加速度As先加速至极限加速度Am，再在当前周期的剩余时间内，按照极限加速度Am匀加速至当前周期结束。
结合第一方面，在第一方面的一种可实现方式中，还包括：
如果中间速度大于极限速度Vm，则对于从手电机在当前周期的运动轨迹，规划先按照第一加加速度从初始速度Vs匀加加速至极限速度Vm，再在当前周期的剩余时间内， 按照极限速度Vm匀速运动至当前周期结束。
结合第一方面，在第一方面的一种可实现方式中，还包括：
如果第二时长大于当前周期的时长，则确定从初始速度Vs，按照第一加加速度，匀加加速到当前周期结束后的末速度；
如果末速度小于极限速度Vm，则对于从手电机在当前周期的运动轨迹，规划从初始速度Vs，按照第一加加速度，匀加加速到当前周期结束。
结合第一方面，在第一方面的一种可实现方式中，还包括：
如果末速度大于极限速度Vm，则对于从手电机在当前周期的运动轨迹，规划先按照第一加加速度，从初始速度Vs匀加加速至极限速度Vm，再在当前周期的剩余时间内，按照极限速度Vm匀速运动至当前周期结束。
结合第一方面，在第一方面的一种可实现方式中，获取从手电机在当前周期内的最大规划距离Pcm，包括：
将从手电机在当前周期内的运动总距离，确定为从手电机在当前周期内的最大规划距离Pcm。
结合第一方面，在第一方面的一种可实现方式中，在第一运动方向与第二运动方向相反的情况下，根据初始运动参数，对从手电机在当前周期的运动轨迹进行规划，包括：
根据初始运动参数，以及第二加加速度，检测从手电机在当前周期内，是否能够从初始速度Vs减速为零；
如果从手电机在当前周期内能够从初始速度Vs减速为零，则获取从手电机从初始速度Vs减速为零时的中间位置和中间加速度；
根据从手电机在中间位置时的运动参数，以及当前周期的剩余时长，对从手电机在当前周期的剩余时长的运动轨迹进行规划；其中，从手电机在中间位置时的初始速度为零、初始加速度为中间加速度。
结合第一方面，在第一方面的一种可实现方式中，还包括：
如果从手电机在当前周期内不能从初始速度Vs减速为零，则检测从手电机在当前周期内是否能够达到减速段最大加速度；
如果从手电机在当前周期内能够达到减速段最大加速度，则对于从手电机在当前周期的运动轨迹，规划先按照第二加加速度减速到减速段最大加速度，再在当前周期的剩余时间内，按照减速段最大加速度，匀减速运动至当前周期结束。
结合第一方面，在第一方面的一种可实现方式中，还包括：
如果从手电机在当前周期内不能够达到减速段最大加速度，则对于从手电机在当前周期的运动轨迹，规划按照第二加加速度，减速运动至当前周期结束。
结合第一方面，在第一方面的一种可实现方式中，获取从手电机在当前周期内的最大规划距离Pcm，包括：
获取从手电机在当前周期结束时的末位置；
将末位置与从手电机在前一周期的实际位置的差值，确定为从手电机在当前周期内的最大规划距离Pcm。
第二方面，本申请实施例提供一种主从手机器人的控制装置，主从手机器人的从手上设置有至少一个从手电机，装置用于对从手电机的运动进行控制；装置包括：
映射位置获取模块，用于获取从手电机在当前周期的映射位置；映射位置用于表示 映射旋转脉冲数，是根据主手的三维坐标以及主从手机器人的机械结构直接映射得到的，主手的三维坐标是按照预设采集周期采集得到的，当前周期为任一采集周期；
初始运动参数获取模块，用于获取从手电机在当前周期的初始运动参数；初始运动参数包括初始速度Vs和初始加速度As；
运动规划模块，用于根据初始运动参数，对从手电机在当前周期的运动轨迹进行规划；
最大规划距离获取模块，用于获取从手电机在当前周期内的最大规划距离Pcm；
目标距离确定模块，用于从从手电机在前一周期的实际位置开始，根据初始运动参数，将映射运动距离和最大规划距离Pcm中，能够优先达到的值，确定为目标距离；映射运动距离为从手电机在当前周期的映射位置与从手电机在前一周期的实际位置的差值，实际位置用于表示实际旋转脉冲数，前一周期为当前周期的前一个采集周期；
实际位置确定模块，用于将从手电机在前一周期的实际位置与目标距离的和，确定为从手电机在当前周期的实际位置。
本申请实施例提供一种主从手机器人的控制方法及装置。该方法中，在主手和从手之间虚拟增加一个虚拟轴，利用虚拟轴获取从手电机在当前周期的映射位置和初始运动参数后，对从手电机在当前周期的运动轨迹进行规划并获取最大规划距离，最终将映射运动距离和最大规划距离中能够优先达到的值确定为从手电机在当前周期需要运动的实际距离。如此，可以避免主手直接映射到从手电机所产生的超出从手电机极限导致电机报错的问题，从而在保证总体轨迹大致不变的情况下，可以将主手运动中过大的加速度、速度，减小至从手电机可接受的程度，延长从手电机的使用寿命。
附图说明
图1为本申请实施例提供的一种主从手机器人的控制方法的工作流程示意图；
图2为本申请实施例提供的主从手机器人的控制方法的步骤S103的一种实现方式的流程示意图；
图3为本申请实施例提供的主从手机器人的控制方法的步骤S103的另一种实现方式的流程示意图；
图4为本申请实施例提供的主从手机器人的控制方法与传统主从手机器人的控制方法的从手电机运动对比示意图；
图5为本申请实施例提供的一种主从手机器人的控制装置的结构示意图。
具体实施方式
为使本申请的目的、技术方案和优点更加清楚，下面将结合附图对本申请实施方式作进一步地详细描述。
为了解决传统控制方法可能出现映射后的从手电机转动加速度超出极限加速度，进而导致电机报错，造成从手无法准确跟随主手进行运动，或者从手的电机寿命受影响的技术问题，本申请通过以下实施例公开了一种主从手机器人的控制方法。本申请实施例提供的控制方法，应用于主从手机器人。主从手机器人包括主手和至少一个从手，每个从手与主手均为一一对应，各个从手之间同步运动。每个从手上均设置有至少一个从手电机。本申请实施例提供的控制方法具体用于对任一从手电机的运动进行控制。为了更清楚地说明本申请实施例，后续未做特殊说明的情况下，以主手仅对应有一个从手，并 且该从手上仅设置有一个从手电机为例，进行具体说明。
在对本申请实施例进行介绍之前，首先对本申请实施例提及的部分概念进行说明。
本申请实施例中提及的主手，可以表示主手上的某一设定点，设定点可以位于掌心位置，或者手指末梢位置，具体可根据经验和实际情况确定，本申请实施例对此不做具体限定。主手的三维坐标，表示设定点的空间三维坐标。本申请实施例中提及的从手，可以表示从手上的目标设定点，比如：从手上的末端点，具体可根据经验和实际情况确定，本申请实施例对此不做具体限定。从手的三维坐标，表示目标设定点的空间三维坐标，与主手的三维坐标处于同一空间坐标系。从手电机的位置表示的是从手电机的旋转脉冲数，从手电机的运动指的是转动，从手电机的速度指的是转动速度，从手电机的加速度指的是转动加速度，从手电机的运动方向包括正转和反转。
下面结合附图对本申请实施例提供的一种主从手机器人的控制方法进行介绍。
参见图1所示的工作流程示意图，本申请实施例提供的一种主从手机器人的控制方法具体包括以下步骤：
步骤S101，获取从手电机在当前周期的映射位置。
其中，映射位置用于表示映射旋转脉冲数，是根据主手的三维坐标以及主从手机器人的机械结构直接映射得到的。主手的三维坐标是按照预设采集周期采集得到的。当前周期为任一采集周期。具体地，采集周期可以根据需要和实际情况进行预设，本申请实施例对此不作具体限定。主手的三维坐标是指在每个采集周期的结束时刻主手位置的空间坐标。从手电机在当前周期的映射位置，是指从手电机在当前周期结束时刻的映射位置。
步骤S102，获取从手电机在当前周期的初始运动参数。
其中，初始运动参数包括初始速度Vs和初始加速度As。
在步骤S102的一种实现方式中，可以首先分别获取从手电机在当前周期以前的三个采集周期的实际位置。接下来，根据从手电机在当前周期以前的三个采集周期的实际位置，确定初始速度Vs和初始加速度As。其中，从手电机在前一周期的第一实际位置，是指从手电机在前一周期结束时刻的实际位置。从手电机在前一周期的前一个采集周期的第二实际位置，是指从手电机在前一周期的前一个采集周期的结束时刻的实际位置。从手电机在前一周期的前一个采集周期的前一个采集周期的第三实际位置，也就是从手电机在当前周期以前的第三个采集周期的第三实际位置，是指从手电机在当前周期以前的第三个采集周期的结束时刻的实际位置。
在确定初始速度Vs和初始加速度As的一种实现方式中，可以通过以下步骤确定初始速度Vs和初始加速度As：
步骤一，根据第一实际位置与第二实际位置的差值，以及采集周期的时长T，确定从手电机在前一周期内的第一平均速度。
具体地，第一平均速度＝(第一实际位置-第二实际位置)/T。
步骤二，根据第二实际位置与第三实际位置的差值，以及采集周期的时长T，确定从手电机在前一周期的前一个采集周期内的第二平均速度。
具体地，第二平均速度＝(第二实际位置-第三实际位置)/T。
步骤三，根据第二平均速度、第一平均速度，以及采集周期的时长T，确定平均加速度。
具体地，平均加速度＝(第一平均速度-第二平均速度)/T。
步骤四，将第一平均速度确定为初始速度Vs，将平均加速度确定为初始加速度As。
在确定初始速度Vs和初始加速度As的其他实现方式中，可以根据实际应用场景，设定其他方式来确定速度和加速度，比如：可以将第一平均速度和第二平均速度的平均值，确定为初始速度Vs。或者，设置初始速度Vs＝第一平均速度+平均加速度×当前周期的时长/2。或者，也可以通过加加速度来得到初始速度Vs和初始加速度As。这样，可以根据实际情况匹配更多的应用场景，从而更具有灵活性。此外，如果初始运动参数已知的情况下，也可以无需根据当前周期的前三个周期的数据进行初始运动参数计算。这样，可以减少三个周期的数据延迟，在任意时刻(比如：要求主手运动过某个位置，或运动具体时间后再进行主从手联动的需求情况下等)应用本申请实施例提供的方法，适应性更广。
另外，在执行步骤S102之后，执行步骤S103之前，本申请实施例提供的控制方法还可以包括：首先，根据从手电机在当前周期的映射位置P4，以及从手电机在前一周期的实际位置P3，确定从手电机在当前周期内的第一运动方向。示例性地，从手电机在当前周期的映射位置P4为10000个脉冲，在前一周期的实际位置P3为5000个脉冲，则确定第一运动方向为正转。从手电机在当前周期的映射位置P4为5000个脉冲，在前一周期的实际位置P3为10000个脉冲，则确定第一运动方向为反转。然后，根据从手电机在前一周期的实际位置P3，以及从手电机在前一周期的前一个采集周期的实际位置P1，确定从手电机在前一周期内的第二运动方向。最后，检测第一运动方向与第二运动方向是否相同。其中，实际位置用于表示实际旋转脉冲数。前一周期为当前周期的前一个采集周期。
步骤S103，根据初始运动参数，对从手电机在当前周期的运动轨迹进行规划。
图2为本申请实施例提供的主从手机器人的控制方法的步骤S103的一种实现方式的流程示意图。在第一运动方向与第二运动方向相同的情况下，如图2所示，步骤S103可以通过以下实现方式基于S形速度曲线进行规划：
步骤S201，检测初始速度Vs是否等于从手电机的极限速度Vm。如果初始速度Vs等于极限速度Vm，则执行步骤S202。如果初始速度Vs小于极限速度Vm，则执行步骤S203。
需要说明的是，如果出现初始速度Vs大于极限速度Vm的情况，则从手电机就会报错，因此通常不会出现初始速度Vs大于极限速度Vm的情况，对于初始速度Vs大于极限速度Vm的情况本申请实施例中不进行单独说明。
步骤S202，对于从手电机在当前周期的运动轨迹，规划按照极限速度Vm匀速运动至当前周期结束。
步骤S203，检测初始加速度As是否等于极限加速度Am。如果初始加速度As等于极限加速度Am，则执行步骤S204。如果初始加速度As小于极限加速度Am，则执行步骤S208。
需要说明的是，如果出现初始加速度As大于极限加速度Am的情况，则从手电机就会报错，因此通常不会出现初始加速度As大于极限加速度Am的情况，对于初始加速度As大于极限加速度Am的情况本申请实施例中不进行单独说明。
步骤S204，确定从初始速度Vs，按照极限加速度Am匀加速到极限速度Vm所需的 第一时长。
具体地，第一时长T1可以通过以下公式确定：
T1＝(Vm-Vs)/Am       公式(1)
步骤S205，检测第一时长是否小于当前周期的时长。如果第一时长小于当前周期的时长，则执行步骤S206。如果第一时长大于当前周期的时长，则执行步骤S207。
另外，如果第一时长等于当前周期的时长，则规划按照极限加速度Am匀加速至极限速度Vm。对于第一时长等于当前周期的时长的情况，不再单独说明。
步骤S206，对于从手电机在当前周期的运动轨迹，规划先按照极限加速度Am匀加速至极限速度Vm，再在当前周期的剩余时间内，按照极限速度Vm匀速运动至当前周期结束。
此时，从手电机可以在当前周期内达到极限速度Vm，因此规划先匀加速运动，再继续匀速运动。
步骤S207，对于从手电机在当前周期的运动轨迹，规划按照极限加速度Am，从初始速度Vs匀加速运动至当前周期结束。
此时，从手电机在当前周期内无法达到极限速度Vm，因此规划按照最大能力匀加速整个周期。
步骤S208，确定从初始加速度As，按照第一加加速度，加速到极限加速度Am所需的第二时长。
具体地，第二时长Ta可以通过以下公式确定：
Ta＝(Am-As)/J1        公式(2)
其中，J1为第一加加速度，表示匀加加速段预设加加速度，J1的数值为正值。
步骤S209，检测第二时长是否小于或等于当前周期的时长。如果第二时长小于或等于当前周期的时长，则执行步骤S210。如果第二时长大于当前周期的时长，则执行步骤S217。
步骤S210，获取第二时长结束后的中间速度。
具体地，第二时长结束后的中间速度V可以通过以下公式确定：
其中，Ta为第二时长，J1为第一加加速度，Vs为初始速度。
步骤S211，检测中间速度是否小于极限速度Vm。如果中间速度小于极限速度Vm，则执行步骤S212。如果中间速度大于极限速度Vm，则执行步骤S216。
另外，如果中间速度等于极限速度Vm，则规划按照第一加加速度从初始速度Vs匀加加速至极限速度Vm。此时，从手电机既达到最大速度，也达到最大加速度。
步骤S212，确定从中间速度，按照极限加速度Am匀加速到极限速度Vm所需的第三时长。
具体地，第三时长T3可以通过以下公式确定：
T3＝(Vm-V)/Am        公式(4)
其中，V为中间速度。
步骤S213，检测第二时长和第三时长的和是否小于当前周期的时长。如果小于，则执行步骤214。如果大于，则执行步骤215。
另外，如果第二时长和第三时长的和等于当前周期的时长，则先按照第一加加速度 加速至极限加速度Am，再按照极限加速度Am匀加速至极限速度Vm。
步骤S214，对于从手电机在当前周期的运动轨迹，规划先按照第一加加速度加速至极限加速度Am，再按照极限加速度Am匀加速至极限速度Vm，最后在当前周期的剩余时间内，按照极限速度Vm匀速运动至当前周期结束。
此时，从手电机在当前周期内，既可以达到极限速度Vm，也可以达到极限加速度Am。
步骤S215，对于从手电机在当前周期的运动轨迹，规划从初始加速度As先加速至极限加速度Am，再在当前周期的剩余时间内，按照极限加速度Am匀加速至当前周期结束。
此时，从手电机在当前周期内，可以达到极限加速度Am，但无法达到极限速度Vm。
步骤S216，对于从手电机在当前周期的运动轨迹，规划先按照第一加加速度从初始速度Vs匀加加速至极限速度Vm，再在当前周期的剩余时间内，按照极限速度Vm匀速运动至当前周期结束。
其中，从初始速度Vs匀加加速至极限速度Vm所需的时长Tv可以通过以下公式确定：
其中，J1为第一加加速度。
此时，从手电机在当前周期内，无法达到极限加速度Am，但可以达到极限速度Vm。
步骤S217，确定从初始速度Vs，按照第一加加速度，匀加加速到当前周期结束后的末速度。
具体地，末速度Ve可以通过以下公式确定：
Ve＝Vs+1/2×J1×T2       公式(6)
其中，J1为第一加加速度，T为当前周期的时长。
步骤S218，检测末速度是否小于或等于极限速度Vm。如果末速度小于或等于极限速度Vm，则执行步骤S219。如果末速度大于极限速度Vm，则执行步骤S216。
步骤S219，对于从手电机在当前周期的运动轨迹，规划从初始速度Vs，按照第一加加速度，匀加加速到当期周期结束。
此时，如果末速度小于极限速度Vm，从手电机在当前周期内，既无法达到极限速度Vm，也无法达到极限加速度Am。
如此，通过上述基于S形速度曲线的轨迹规划方式，可以使得从手电机的速度较为平缓，且不报错，速度和加速度均可以满足系统电机的参数要求，而且运动轨迹可以大致追随主手的运动轨迹。
图3为本申请实施例提供的主从手机器人的控制方法的步骤S103的另一种实现方式的流程示意图。在第一运动方向与第二运动方向相反的情况下，如图3所示，步骤S103可以通过以下实现方式实现：
步骤S301，根据初始运动参数，以及第二加加速度，检测从手电机在当前周期内，是否能够从初始速度Vs减速为零。如果能够从初始速度Vs减速为零，则执行步骤S302。如果不能从初始速度Vs减速为零，则执行步骤S304。
其中，第二加加速度，表示匀加减速段预设加加速度，且数值为负值。
步骤S302，获取从手电机从初始速度Vs减速为零时的中间位置和中间加速度。
具体地，根据从手电机的初始运动参数，以及到达中间位置时的速度，再结合第二加加速度，运动学分析后可以求解出中间位置和中间加速度。
步骤S303，根据从手电机在中间位置时的运动参数，以及当前周期的剩余时长，对从手电机在当前周期的剩余时长的运动轨迹进行规划。其中，从手电机在中间位置时的初始速度为零、初始加速度为中间加速度。
具体地，运动轨迹规划方式可以参照前述步骤S201至步骤S219，此处不再单独赘述。
步骤S304，检测从手电机在当前周期内是否能够达到减速段最大加速度。如果能，则执行步骤S305，否则，执行步骤S306。
具体地，可以首先确定从手电机从初始运动参数按照第二加加速度减速到减速段最大加速度时所需的时长，如果该时长大于当前周期的时长，则确定从手电机在当前周期内无法达到减速段最大加速度，否则，确定从手电机在当前周期内能够达到减速段最大加速度。
步骤S305，对于从手电机在当前周期的运动轨迹，规划先按照第二加加速度减速到减速段最大加速度，再在当前周期的剩余时间内，按照减速段最大加速度，匀减速运动至当前周期结束。
具体地，从初始加速度As按照第二加加速度减速到减速段最大加速度，所需的第四时长T4可以通过以下公式确定：
T4＝(Asmd-As)/J2          公式(7)
其中，J2为第二加加速度，表示匀加减速段预设加加速度，且数值为负值。Asmd为减速段最大加速度。
第四时长T4结束后的中间速度V2可以通过以下公式确定：
其中，T4为第四时长，J2为第二加加速度，Vs为初始速度。
步骤S306，对于从手电机在当前周期的运动轨迹，规划按照第二加加速度，减速运动至当前周期结束。
如此，通过上述基于S形速度曲线的轨迹规划方式，可以使得从手电机的速度较为平缓，且不报错，速度和加速度均可以满足系统电机的参数要求，而且运动轨迹可以大致追随主手的运动轨迹。
此外，还可以通过其他方式进行轨迹规划，比如：基于T形速度曲线进行规划。如此，轨迹规划可以更灵活，从而可以具有更广泛的应用范围。
需要说明的是，本申请实施例中，在当前周期为初始采集周期时，将从手电机在初始采集周期的初始速度和初始加速度均设置为零，无需进行运动规划。如果当前周期为初始采集周期之后的采集周期，则可以执行步骤S103来进行运动轨迹规划。其中，如果当前周期为初始采集周期之后的第一个周期，则无需进行运动方向检测，直接默认为运动方向与前一周期的方向相同，并且初始速度和初始加速度均为零。如果当前周期为初始采集周期之后的第一个周期以后的周期，则需要首先进行运动方向检测。
步骤S104，获取从手电机在当前周期内的最大规划距离Pcm。
具体地，步骤S104与前述各种规划方式相对应。
在一种实现方式中，在第一运动方向与第二运动方向相同的情况下，按照上述步骤S201至步骤S219，对从手电机在当前周期的运动轨迹进行规划以后，可以将从手电机在当前周期内的运动总距离，确定为从手电机在当前周期内的最大规划距离Pcm。
下面对最大规划距离Pcm的计算进行具体介绍。
在执行步骤S202以后，最大规划距离Pcm可以通过以下公式进行确定：
Pcm＝Vm×T         公式(9)
其中，Vm为极限速度，T为当前周期的时长。
此外，在执行步骤S206以后，最大规划距离Pcm可以通过以下公式进行确定：
其中，Vs为初始速度，T1为第一时长，Am为极限加速度，Vm为极限速度，T为当前周期的时长。
此外，在执行步骤S207以后，最大规划距离Pcm可以通过以下公式进行确定：
Pcm＝Vs×T+1/2×Am×T2          公式(11)
其中，Vs为初始速度，Am为极限加速度，T为当前周期的时长。
此外，在执行步骤S214以后，最大规划距离Pcm可以通过以下公式进行确定：
其中，J1为第一加加速度，Ta为第二时长，T3为第三时长，T为当前周期的时长，V为第二时长结束后的中间速度，Vs为初始速度，Vm为极限速度。
此外，在执行步骤S215以后，最大规划距离Pcm可以通过以下公式进行确定：
其中，J1为第一加加速度，Ta为第二时长，T为当前周期的时长，V为第二时长结束后的中间速度，Vs为初始速度，Am为极限加速度。
此外，在执行步骤S216以后，最大规划距离Pcm可以通过以下公式进行确定：
其中，J1为第一加加速度，Vs为初始速度，Vm为极限速度，T为当前周期的时长，Tv为从初始速度Vs匀加加速至极限速度Vm所需的时长，具体可通过公式(5)确定。
此外，在执行步骤S219以后，最大规划距离Pcm可以通过以下公式进行确定：
Pcm＝1/6×J1×T3+Vs×T          公式(15)
其中，J1为第一加加速度，Vs为初始速度，T为当前周期的时长。
在另一种实现方式中，在第一运动方向与第二运动方向相反的情况下，按照上述步骤S301至步骤S306，对从手电机在当前周期的运动轨迹进行规划以后，可以首先获取从手电机在当前周期结束时的末位置，然后将末位置与从手电机在前一周期的实际位置的差值，确定为从手电机在当前周期内的最大规划距离Pcm。具体地，末位置等于从手电机的初始位置(即在前一周期的实际位置)加上当前周期的运动总距离。需要说明的是，由于涉及反向运动，因此运动距离是具有方向性的矢量。
下面对末位置的计算进行具体介绍。
在执行步骤S303之后，从手电机是首先维持与初始速度相同的方向进行减速运动，直至速度减为零，然后反向运动，直至当前周期结束。末位置是从手电机的初始位置加上减速段的运动总距离后，再减去反向运动的运动总距离所得到的结果。对于反向运动的运动轨迹规划，可参照前述步骤S201至步骤S219规划，对于反向运动的运动总距离，可以参照步骤S104的一种实现方式中所对应的最大规划距离计算方式进行计算，此处不 再赘述。需要说明的是，反向运动的时长为当前周期的剩余时长，且反向运动的初始速度为零。
在执行步骤S305和步骤S306之后，从手电机是维持与初始速度相同的方向进行减速运动，直至当前周期结束。此时，末位置是从手电机的初始位置加上最大规划距离Pcm所得到的结果。
具体地，在执行步骤S305之后，最大规划距离Pcm可以通过以下公式进行确定：
其中，J2为第二加加速度，Vs为初始速度，T为当前周期的时长，T4为第四时长，V2为第四时长T4结束后的中间速度，Asmd为减速段最大加速度。
在执行步骤S306之后，最大规划距离Pcm可以通过以下公式进行确定：
Pcm＝1/6＝J2×T3+Vs×T           公式(17)
其中，J2为第一加加速度，Vs为初始速度，T为当前周期的时长。
步骤S105，从从手电机在前一周期的实际位置开始，根据初始运动参数，将映射运动距离和最大规划距离Pcm中，能够优先达到的值，确定为目标距离。
其中，映射运动距离为从手电机在当前周期的映射位置与从手电机在前一周期的实际位置的差值。实际位置用于表示实际旋转脉冲数。前一周期为当前周期的前一个采集周期。
示例性地，从手电机在前一周期的实际位置为5000个脉冲，从手电机在当前周期的映射位置为2000个脉冲，映射运动距离为-3000个脉冲。根据从手电机的初始运动参数进行运动轨迹规划后，从手电机首先沿与初始速度相同的方向继续减速运动，直至速度减为零，此时中间位置为6500个脉冲，然后反向运动至当前周期结束，最后末位置为3000个脉冲，最大规划距离Pcm为-2000个脉冲。此时，由于从手电机需要反向运动，因此必要要把速度减为零后才能反向运动，从而从5000个脉冲反向运动后，能够优先达到的值为-2000个脉冲，因此，确定目标距离为-2000个脉冲。又示例性地，从手电机在前一周期的实际位置为5000个脉冲，从手电机在当前周期的映射位置为2000个脉冲，映射运动距离为-3000个脉冲。从手电机规划的运动轨迹是沿与初始速度相同的方向一直减速运动到当前周期结束，此时速度仍未减为零，且末位置为8000个脉冲，最大规划距离Pcm为3000个脉冲。根据电机的运动可知，能够优先达到的值为8000个脉冲，因此，确定目标距离为3000个脉冲。需要说明的是，本申请实施例中，映射运动距离和最大规划距离Pcm的符号均用于表示方向。
步骤S106，将从手电机在前一周期的实际位置与目标距离的和，确定为从手电机在当前周期的实际位置。
如此，本申请实施例提供的控制方法，相当于在主手和从手之间增加了一个虚拟轴，通过虚拟轴对数据进行处理后，将映射运动距离和最大规划距离中能够优先达到的值发送给从手电机，如此，可以避免主手直接映射到从手电机所产生的超出从手电机极限导致电机报错的问题，从而在保证总体轨迹大致不变的情况下，将主手运动中过大的加速度、速度，减小至从手电机可接受的程度，延长从手电机的使用寿命。
在执行步骤S106之后，本申请实施例提供的控制方法还可以包括：
步骤S107，根据从手电机在当前周期的实际位置，确定从手在当前周期的实际三维坐标。
步骤S108，检测实际三维坐标是否超过从手的极限坐标。如果超过，则执行步骤S109。如果未超过，则执行步骤S110。
步骤S109，根据从手的极限坐标，获取从手电机在当前周期的修正位置。
在检测实际三维坐标是否超过从手的极限坐标之后，无论是否对从手电机在的当前周期的实际位置进行修正，均继续执行如下步骤：
步骤S110，检测主从手机器人的各个从手在当前周期的多轴运动是否同步。如果不同步，则执行步骤S111。如果同步，则执行步骤S116。
步骤S111，根据各个从手在当前周期的目标三维坐标，以及主从手机器人的机械结构，利用运动学逆解的方法获取各个从手上的从手电机的目标位置。
步骤S112，根据各个从手上的从手电机的目标位置，获取主手的目标三维坐标。
步骤S113，检测主手的目标三维坐标是否超出主手的极限坐标。如果主手的目标三维坐标超出主手的极限坐标，则执行步骤S114。如果主手的目标三维坐标未超出主手的极限坐标，则执行步骤S115。
步骤S114，将主手在当前周期的三维坐标修正为主手的极限坐标。然后返回步骤S101重新获取各个从手电机在当前周期的映射位置。
步骤S115，将主手在当前周期的三维坐标修正为主手的目标三维坐标。然后返回步骤S101重新获取各个从手电机在当前周期的映射位置。
步骤S116，检测从手电机在当前周期的实际位置是否经过修正。如果未经过修正，则执行步骤S117。如果经过修正，则执行步骤S118。
步骤S117，控制从手电机运动到当前周期的实际位置。
步骤S118，控制从手电机运动到当前周期的修正位置。
如此，通过上述方式，在实际应用中如发现影响从手多轴协同，可通过机器人运动正逆解来得到主手的运动限制参数，基于主手的运动限制参数再重新进行本申请实施例提供的虚拟轴算法处理，最终映射到从手各关节，可以有效避免特殊情况下从手联动出现的多轴协同问题。
图4为本申请实施例提供的主从手机器人的控制方法与传统主从手机器人的控制方法的从手电机运动对比示意图。如图4所示，各个曲线的横轴都为采样周期点。其中，在运动距离变化曲线中，曲线A1为采用传统主从手机器人的控制方法(即从主手直接映射到从手电机)所得到的从手电机的运动距离变化曲线；曲线B1为采用本申请实施例提供的主从手机器人的控制方法所得到的从手电机的运动距离变化曲线。曲线A1是直接根据主手的运动映射得到的运动轨迹，但由于电机能力不足，从手电机无法实现该段轨迹。经本申请实施例中的虚拟轴处理方法进行处理后，曲线B1中从手电机大致追随主手的运动轨迹，在斜率(即运动速度)上，相比曲线A1的原始数据有所降低，以适应从手电机的实际运动能力，避免报错。在运动速度变化曲线中，曲线A2为采用传统主从手机器人的控制方法所得到的从手电机的运动速度变化曲线；曲线B2为采用本申请实施例提供的主从手机器人的控制方法所得到的从手电机的运动速度变化曲线。曲线A2中速度是离散的，且突变明显，速度经常会很大，而曲线B2中的速度则较为平缓，在到达从手电机极限值时，不再上升，保持匀速运动。在加速度变化曲线中，曲线A3为采用传统主从手机 器人的控制方法所得到的从手电机的加速度变化曲线；曲线B3为采用本申请实施例提供的主从手机器人的控制方法所得到的从手电机的加速度变化曲线。曲线A3中加速度非常大，从手电机通常无法达到。而曲线B3中加速度变化很小，仅在变向时有一些突变，相比曲线A3中的数据，电机将运行更平稳，且不报错。
下述为本申请的装置实施例，可以用于执行本申请的方法实施例。对于本申请装置实施例中未披露的细节，请参照本申请的方法实施例。
作为对上述各实施例的实现，本申请实施例提供一种主从手机器人的控制装置。其中，主从手机器人的从手上设置有至少一个从手电机，该控制装置用于对从手电机的运动进行控制。参见图5所示的结构示意图，本申请实施例提供的主从手机器人的控制装置包括：映射位置获取模块501、初始运动参数获取模块502、运动规划模块503、最大规划距离获取模块504、目标距离确定模块505和实际位置确定模块506。此外，所述装置还可以包括从手位置检测模块和从手运动控制模块等更多或更少的单元和模块，本实施例对该装置的结构不做限制。
映射位置获取模块501，用于获取从手电机在当前周期的映射位置；映射位置用于表示映射旋转脉冲数，是根据主手的三维坐标以及主从手机器人的机械结构直接映射得到的，主手的三维坐标是按照预设采集周期采集得到的，当前周期为任一采集周期；
初始运动参数获取模块502，用于获取从手电机在当前周期的初始运动参数；初始运动参数包括初始速度Vs和初始加速度As；
运动规划模块503，用于根据初始运动参数，对从手电机在当前周期的运动轨迹进行规划；
最大规划距离获取模块504，用于获取从手电机在当前周期内的最大规划距离Pcm；
目标距离确定模块505，用于从从手电机在前一周期的实际位置开始，根据初始运动参数，将映射运动距离和最大规划距离Pcm中，能够优先达到的值，确定为目标距离；映射运动距离为从手电机在当前周期的映射位置与从手电机在前一周期的实际位置的差值，实际位置用于表示实际旋转脉冲数，前一周期为当前周期的前一个采集周期；
实际位置确定模块506，用于将从手电机在前一周期的实际位置与目标距离的和，确定为从手电机在当前周期的实际位置。
在一种可实现方式中，该装置还包括：
从手位置检测模块，用于根据从手电机在当前周期的实际位置，确定从手在当前周期的实际三维坐标；以及，检测实际三维坐标是否超过从手的极限坐标；
从手运动控制模块，用于如果实际三维坐标未超过从手的极限坐标，则控制从手电机运动到当前周期的实际位置。
在一种可实现方式中，从手运动控制模块还用于：如果实际三维坐标超过从手的极限坐标，则根据从手的极限坐标，获取从手电机在当前周期的修正位置；以及，控制从手电机运动到当前周期的修正位置。
在一种可实现方式中，从手运动控制模块在控制从手电机运动之前，还用于：
如果主从手机器人的各个从手在当前周期的多轴运动不同步，则根据各个从手在当前周期的目标三维坐标，以及主从手机器人的机械结构，利用运动学逆解的方法获取各个从手上的从手电机的目标位置；根据各个从手上的从手电机的目标位置，获取主手的目标三维坐标；如果主手的目标三维坐标超出主手的极限坐标，则将主手在当前周期的 三维坐标修正为主手的极限坐标；根据主手的极限坐标，重新获取从手电机在当前周期的映射位置，直至重新确定出从手电机在当前周期的实际位置。
在一种可实现方式中，从手运动控制模块还用于：
如果主手的目标三维坐标未超出主手的极限坐标，则将主手在当前周期的三维坐标修正为主手的目标三维坐标，并发送回映射位置获取模块501。
在一种可实现方式中，初始运动参数获取模块502具体用于：
分别获取从手电机在当前周期以前的三个采集周期的实际位置；根据从手电机在当前周期以前的三个采集周期的实际位置，确定初始速度Vs和初始加速度As。
在一种可实现方式中，初始运动参数获取模块502具体用于：
根据第一实际位置与第二实际位置的差值，以及采集周期的时长，确定从手电机在前一周期内的第一平均速度；第一实际位置为从手电机在前一周期的实际位置，第二实际位置为从手电机在前一周期的前一个采集周期的实际位置；根据第二实际位置与第三实际位置的差值，以及采集周期的时长，确定从手电机在前一周期的前一个采集周期内的第二平均速度；第三实际位置为从手电机在当前周期以前的第三个采集周期的实际位置；根据第二平均速度、第一平均速度，以及采集周期的时长，确定平均加速度；将第一平均速度确定为初始速度Vs，将平均加速度确定为初始加速度As。
在一种可实现方式中，该装置在运动规划模块503之前，还包括：
方向检测模块，用于根据从手电机在当前周期的映射位置，以及从手电机在前一周期的实际位置，确定从手电机在当前周期内的第一运动方向；根据从手电机在前一周期的实际位置，以及从手电机在前一周期的前一个采集周期的实际位置，确定从手电机在前一周期内的第二运动方向；检测第一运动方向与第二运动方向是否相同。
在一种可实现方式中，在第一运动方向与第二运动方向相同的情况下，运动规划模块503具体用于：
检测初始速度Vs是否等于从手电机的极限速度Vm；如果初始速度Vs等于极限速度Vm，则对于从手电机在当前周期的运动轨迹，规划按照极限速度Vm匀速运动至当前周期结束。
在一种可实现方式中，运动规划模块503具体还用于：
如果初始速度Vs小于极限速度Vm，则检测初始加速度As是否等于极限加速度Am；如果初始加速度As等于极限加速度Am，则确定从初始速度Vs，按照极限加速度Am匀加速到极限速度Vm所需的第一时长；如果第一时长小于当前周期的时长，则对于从手电机在当前周期的运动轨迹，规划先按照极限加速度Am匀加速至极限速度Vm，再在当前周期的剩余时间内，按照极限速度Vm匀速运动至当前周期结束。
在一种可实现方式中，运动规划模块503具体还用于：
如果第一时长大于当前周期的时长，则对于从手电机在当前周期的运动轨迹，规划按照极限加速度Am，从初始速度Vs匀加速运动至当前周期结束。
在一种可实现方式中，运动规划模块503具体还用于：
如果初始加速度As小于极限加速度Am，则确定从初始加速度As，按照第一加加速度，加速到极限加速度Am所需的第二时长；如果第二时长小于当前周期的时长，则获取第二时长结束后的中间速度；如果中间速度小于极限速度Vm，则确定从中间速度，按照极限加速度Am匀加速到极限速度Vm所需的第三时长；如果第二时长和第三时长的和小 于当前周期的时长，则对于从手电机在当前周期的运动轨迹，规划先按照第一加加速度加速至极限加速度Am，再按照极限加速度Am匀加速至极限速度Vm，最后在当前周期的剩余时间内，按照极限速度Vm匀速运动至当前周期结束。
在一种可实现方式中，运动规划模块503具体还用于：
如果第二时长和第三时长的和大于当前周期的时长，则对于从手电机在当前周期的运动轨迹，规划从初始加速度As先加速至极限加速度Am，再在当前周期的剩余时间内，按照极限加速度Am匀加速至当前周期结束。
在一种可实现方式中，运动规划模块503具体还用于：
如果中间速度大于极限速度Vm，则对于从手电机在当前周期的运动轨迹，规划先按照第一加加速度从初始速度Vs匀加加速至极限速度Vm，再在当前周期的剩余时间内，按照极限速度Vm匀速运动至当前周期结束。
在一种可实现方式中，运动规划模块503具体还用于：
如果第二时长大于当前周期的时长，则确定从初始速度Vs，按照第一加加速度，匀加加速到当前周期结束后的末速度；如果末速度小于极限速度Vm，则对于从手电机在当前周期的运动轨迹，规划从初始速度Vs，按照第一加加速度，匀加加速到当前周期结束。
在一种可实现方式中，运动规划模块503具体还用于：
如果末速度大于极限速度Vm，则对于从手电机在当前周期的运动轨迹，规划先按照第一加加速度，从初始速度Vs匀加加速至极限速度Vm，再在当前周期的剩余时间内，按照极限速度Vm匀速运动至当前周期结束。
在一种可实现方式中，最大规划距离获取模块504具体用于：
将从手电机在当前周期内的运动总距离，确定为从手电机在当前周期内的最大规划距离Pcm。
在一种可实现方式中，在第一运动方向与第二运动方向相反的情况下，运动规划模块503具体用于：
根据初始运动参数，以及第二加加速度，检测从手电机在当前周期内，是否能够从初始速度Vs减速为零；如果从手电机在当前周期内能够从初始速度Vs减速为零，则获取从手电机从初始速度Vs减速为零时的中间位置和中间加速度；根据从手电机在中间位置时的运动参数，以及当前周期的剩余时长，对从手电机在当前周期的剩余时长的运动轨迹进行规划；其中，从手电机在中间位置时的初始速度为零、初始加速度为中间加速度。
在一种可实现方式中，运动规划模块503具体还用于：
如果从手电机在当前周期内不能从初始速度Vs减速为零，则检测从手电机在当前周期内是否能够达到减速段最大加速度；如果从手电机在当前周期内能够达到减速段最大加速度，则对于从手电机在当前周期的运动轨迹，规划先按照第二加加速度减速到减速段最大加速度，再在当前周期的剩余时间内，按照减速段最大加速度，匀减速运动至当前周期结束。
在一种可实现方式中，运动规划模块503具体还用于：
如果从手电机在当前周期内不能够达到减速段最大加速度，则对于从手电机在当前周期的运动轨迹，规划按照第二加加速度，减速运动至当前周期结束。
在一种可实现方式中，最大规划距离获取模块504具体用于：
获取从手电机在当前周期结束时的末位置；将末位置与从手电机在前一周期的实际位置的差值，确定为从手电机在当前周期内的最大规划距离Pcm。
如此，本申请实施例提供的控制装置，相当于在主手和从手之间增加了一个虚拟轴，通过虚拟轴对数据进行处理后，将映射运动距离和最大规划距离中能够优先达到的值发送给从手电机，如此，可以避免主手直接映射到从手电机所产生的超出从手电机极限导致电机报错的问题，从而在保证总体轨迹大致不变的情况下，将主手运动中过大的加速度、速度，减小至从手电机可接受的程度，延长从手电机的使用寿命。

Claims (22)

  1. 一种主从手机器人的控制方法，其特征在于，所述主从手机器人的从手上设置有至少一个从手电机，所述方法用于对所述从手电机的运动进行控制；所述方法包括：
    获取所述从手电机在当前周期的映射位置；所述映射位置用于表示映射旋转脉冲数，是根据所述主从手机器人的主手的三维坐标以及所述主从手机器人的机械结构直接映射得到的，所述主手的三维坐标是按照预设采集周期采集得到的，所述当前周期为任一采集周期；
    获取所述从手电机在所述当前周期的初始运动参数；所述初始运动参数包括初始速度Vs和初始加速度As；
    根据所述初始运动参数，对所述从手电机在所述当前周期的运动轨迹进行规划；
    获取所述从手电机在所述当前周期内的最大规划距离Pcm；
    从所述从手电机在前一周期的实际位置开始，根据所述初始运动参数，将映射运动距离和所述最大规划距离Pcm中，能够优先达到的值，确定为目标距离；所述映射运动距离为所述从手电机在所述当前周期的映射位置与所述从手电机在前一周期的实际位置的差值，所述实际位置用于表示实际旋转脉冲数，所述前一周期为所述当前周期的前一个采集周期；
    将所述从手电机在前一周期的实际位置与所述目标距离的和，确定为所述从手电机在所述当前周期的实际位置。
  2. 根据权利要求1所述的方法，其特征在于，还包括：
    根据所述从手电机在所述当前周期的实际位置，确定所述从手在所述当前周期的实际三维坐标；
    检测所述实际三维坐标是否超过所述从手的极限坐标；
    如果所述实际三维坐标未超过所述从手的极限坐标，则控制所述从手电机运动到所述当前周期的实际位置。
  3. 根据权利要求2所述的方法，其特征在于，还包括：
    如果所述实际三维坐标超过所述从手的极限坐标，则根据所述从手的极限坐标，获取所述从手电机在所述当前周期的修正位置；
    控制所述从手电机运动到所述当前周期的修正位置。
  4. 根据权利要求2或3所述的方法，其特征在于，在控制所述从手电机运动之前，还包括：
    如果所述主从手机器人的各个从手在所述当前周期的多轴运动不同步，则根据各个从手在所述当前周期的目标三维坐标，以及所述主从手机器人的机械结构，利用运动学逆解的方法获取各个从手上的从手电机的目标位置；
    根据各个从手上的从手电机的目标位置，获取所述主手的目标三维坐标；
    如果所述主手的目标三维坐标超出所述主手的极限坐标，则将所述主手在所述当前周期的三维坐标修正为所述主手的极限坐标；
    根据所述主手的极限坐标，重新获取从手电机在当前周期的映射位置，直至重新确定出所述从手电机在所述当前周期的实际位置。
  5. 根据权利要求4所述的方法，其特征在于，还包括：
    如果所述主手的目标三维坐标未超出所述主手的极限坐标，则将所述主手在所述当前周期的三维坐标修正为所述主手的目标三维坐标；
    根据所述主手的目标三维坐标，重新获取从手电机在当前周期的映射位置，直至重新确定出所述从手电机在所述当前周期的实际位置。
  6. 根据权利要求1所述的方法，其特征在于，获取所述从手电机在所述当前周期的初始运动参数，包括：
    分别获取所述从手电机在所述当前周期以前的三个采集周期的实际位置；
    根据所述从手电机在所述当前周期以前的三个采集周期的实际位置，确定所述初始速度Vs和所述初始加速度As。
  7. 根据权利要求6所述的方法，其特征在于，所述根据所述从手电机在所述当前周期以前的三个采集周期的实际位置，确定所述初始速度Vs和所述初始加速度As，包括：
    根据第一实际位置与第二实际位置的差值，以及所述采集周期的时长，确定所述从手电机在所述前一周期内的第一平均速度；所述第一实际位置为所述从手电机在所述前一周期的实际位置，所述第二实际位置为所述从手电机在所述前一周期的前一个采集周期的实际位置；
    根据所述第二实际位置与第三实际位置的差值，以及所述采集周期的时长，确定所述从手电机在所述前一周期的前一个采集周期内的第二平均速度；所述第三实际位置为所述从手电机在所述当前周期以前的第三个采集周期的实际位置；
    根据所述第二平均速度、所述第一平均速度，以及所述采集周期的时长，确定平均加速度；
    将所述第一平均速度确定为所述初始速度Vs，将所述平均加速度确定为所述初始加速度As。
  8. 根据权利要求1所述的方法，其特征在于，在根据所述初始运动参数，对所述从手电机在所述当前周期的运动轨迹进行规划之前，所述方法还包括：
    根据所述从手电机在所述当前周期的映射位置，以及所述从手电机在所述前一周期的实际位置，确定所述从手电机在所述当前周期内的第一运动方向；
    根据所述从手电机在所述前一周期的实际位置，以及所述从手电机在所述前一周期的前一个采集周期的实际位置，确定所述从手电机在所述前一周期内的第二运动方向；
    检测所述第一运动方向与所述第二运动方向是否相同。
  9. 根据权利要求8所述的方法，其特征在于，在所述第一运动方向与所述第二运动方向相同的情况下，所述根据所述初始运动参数，对所述从手电机在所述当前周期的运动轨迹进行规划，包括：
    检测所述初始速度Vs是否等于所述从手电机的极限速度Vm；
    如果所述初始速度Vs等于所述极限速度Vm，则对于所述从手电机在所述当前周期的运动轨迹，规划按照所述极限速度Vm匀速运动至所述当前周期结束。
  10. 根据权利要求9所述的方法，其特征在于，还包括：
    如果所述初始速度Vs小于所述极限速度Vm，则检测所述初始加速度As是否等于所述极限加速度Am；
    如果所述初始加速度As等于所述极限加速度Am，则确定从所述初始速度Vs，按照所述极限加速度Am匀加速到所述极限速度Vm所需的第一时长；
    如果所述第一时长小于所述当前周期的时长，则对于所述从手电机在所述当前周期的运动轨迹，规划先按照所述极限加速度Am匀加速至所述极限速度Vm，再在所述当前周期的剩余时间内，按照所述极限速度Vm匀速运动至所述当前周期结束。
  11. 根据权利要求10所述的方法，其特征在于，还包括：
    如果所述第一时长大于所述当前周期的时长，则对于所述从手电机在所述当前周期的运动轨迹，规划按照所述极限加速度Am，从所述初始速度Vs匀加速运动至所述当前周期结束。
  12. 根据权利要求10所述的方法，其特征在于，还包括：
    如果所述初始加速度As小于所述极限加速度Am，则确定从所述初始加速度As，按照第一加加速度，加速到所述极限加速度Am所需的第二时长；
    如果所述第二时长小于所述当前周期的时长，则获取所述第二时长结束后的中间速度；
    如果所述中间速度小于所述极限速度Vm，则确定从所述中间速度，按照所述极限加速度Am匀加速到所述极限速度Vm所需的第三时长；
    如果所述第二时长和所述第三时长的和小于所述当前周期的时长，则对于所述从手电机在所述当前周期的运动轨迹，规划先按照所述第一加加速度加速至所述极限加速度Am，再按照所述极限加速度Am匀加速至所述极限速度Vm，最后在所述当前周期的剩余时间内，按照所述极限速度Vm匀速运动至所述当前周期结束。
  13. 根据权利要求12所述的方法，其特征在于，还包括：
    如果所述第二时长和所述第三时长的和大于所述当前周期的时长，则对于所述从手电机在所述当前周期的运动轨迹，规划从所述初始加速度As先加速至所述极限加速度Am，再在所述当前周期的剩余时间内，按照所述极限加速度Am匀加速至所述当前周期结束。
  14. 根据权利要求12所述的方法，其特征在于，还包括：
    如果所述中间速度大于所述极限速度Vm，则对于所述从手电机在所述当前周期的运动轨迹，规划先按照所述第一加加速度从所述初始速度Vs匀加加速至所述极限速度Vm，再在所述当前周期的剩余时间内，按照所述极限速度Vm匀速运动至所述当前周期结束。
  15. 根据权利要求12所述的方法，其特征在于，还包括：
    如果所述第二时长大于所述当前周期的时长，则确定从所述初始速度Vs，按照所述第一加加速度，匀加加速到所述当前周期结束后的末速度；
    如果所述末速度小于所述极限速度Vm，则对于所述从手电机在所述当前周期的运动轨迹，规划从所述初始速度Vs，按照所述第一加加速度，匀加加速到所述当前周期结束。
  16. 根据权利要求15所述的方法，其特征在于，还包括：
    如果所述末速度大于所述极限速度Vm，则对于所述从手电机在所述当前周期的运动轨迹，规划先按照所述第一加加速度，从所述初始速度Vs匀加加速至所述极限速度Vm，再在所述当前周期的剩余时间内，按照所述极限速度Vm匀速运动至所述当前周期结束。
  17. 根据权利要求9-16任一项所述的方法，其特征在于，所述获取所述从手电机在所述当前周期内的最大规划距离Pcm，包括：
    将所述从手电机在所述当前周期内的运动总距离，确定为所述从手电机在所述当前周期内的最大规划距离Pcm。
  18. 根据权利要求8所述的方法，其特征在于，在所述第一运动方向与所述第二运动方向相反的情况下，所述根据所述初始运动参数，对所述从手电机在所述当前周期的运动轨迹进行规划，包括：
    根据所述初始运动参数，以及第二加加速度，检测所述从手电机在所述当前周期内，是否能够从所述初始速度Vs减速为零；
    如果所述从手电机在所述当前周期内能够从所述初始速度Vs减速为零，则获取所述从手电机从所述初始速度Vs减速为零时的中间位置和中间加速度；
    根据所述从手电机在所述中间位置时的运动参数，以及所述当前周期的剩余时长，对所述从手电机在所述当前周期的剩余时长的运动轨迹进行规划；其中，所述从手电机在所述中间位置时的初始速度为零、初始加速度为所述中间加速度。
  19. 根据权利要求18所述的方法，其特征在于，还包括：
    如果所述从手电机在所述当前周期内不能从所述初始速度Vs减速为零，则检测所述从手电机在所述当前周期内是否能够达到减速段最大加速度；
    如果所述从手电机在所述当前周期内能够达到减速段最大加速度，则对于所述从手电机在所述当前周期的运动轨迹，规划先按照所述第二加加速度减速到所述减速段最大加速度，再在所述当前周期的剩余时间内，按照所述减速段最大加速度，匀减速运动至所述当前周期结束。
  20. 根据权利要求19所述的方法，其特征在于，还包括：
    如果所述从手电机在所述当前周期内不能够达到减速段最大加速度，则对于所述从手电机在所述当前周期的运动轨迹，规划按照所述第二加加速度，减速运动至所述当前周期结束。
  21. 根据权利要求18-20任一项所述的方法，其特征在于，所述获取所述从手电机在所述当前周期内的最大规划距离Pcm，包括：
    获取所述从手电机在所述当前周期结束时的末位置；
    将所述末位置与所述从手电机在前一周期的实际位置的差值，确定为所述从手电机在所述当前周期内的最大规划距离Pcm。
  22. 一种主从手机器人的控制装置，其特征在于，所述主从手机器人的从手上设置有至少一个从手电机，所述装置用于对所述从手电机的运动进行控制；所述装置包括：
    映射位置获取模块，用于获取所述从手电机在当前周期的映射位置；所述映射位置用于表示映射旋转脉冲数，是根据主手的三维坐标以及所述主从手机器人的机械结构直接映射得到的，所述主手的三维坐标是按照预设采集周期采集得到的，所述当前周期为任一采集周期；
    初始运动参数获取模块，用于获取所述从手电机在所述当前周期的初始运动参数；所述初始运动参数包括初始速度Vs和初始加速度As；
    运动规划模块，用于根据所述初始运动参数，对所述从手电机在所述当前周期的运动轨迹进行规划；
    最大规划距离获取模块，用于获取所述从手电机在所述当前周期内的最大规划距离Pcm；
    目标距离确定模块，用于从所述从手电机在前一周期的实际位置开始，根据所述初始运动参数，将映射运动距离和所述最大规划距离Pcm中，能够优先达到的值，确定为 目标距离；所述映射运动距离为所述从手电机在所述当前周期的映射位置与所述从手电机在前一周期的实际位置的差值，所述实际位置用于表示实际旋转脉冲数，所述前一周期为所述当前周期的前一个采集周期；
    实际位置确定模块，用于将所述从手电机在前一周期的实际位置与所述目标距离的和，确定为所述从手电机在所述当前周期的实际位置。
PCT/CN2023/078377 2022-07-29 2023-02-27 一种主从手机器人的控制方法及装置 WO2024021599A1 (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN202210910678.8 2022-07-29
CN202210910678.8A CN115847371B (zh) 2022-07-29 2022-07-29 一种主从手机器人的控制方法及装置

Publications (1)

Publication Number Publication Date
WO2024021599A1 true WO2024021599A1 (zh) 2024-02-01

Family

ID=85660406

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2023/078377 WO2024021599A1 (zh) 2022-07-29 2023-02-27 一种主从手机器人的控制方法及装置

Country Status (2)

Country Link
CN (1) CN115847371B (zh)
WO (1) WO2024021599A1 (zh)

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH04372380A (ja) * 1991-06-17 1992-12-25 Hitachi Ltd マニピュレ−タの操作装置
CN106003034A (zh) * 2016-06-16 2016-10-12 深圳先进技术研究院 一种主从式机器人控制系统及控制方法
CN106137400A (zh) * 2016-05-31 2016-11-23 微创（上海）医疗机器人有限公司 用于机械臂的控制系统、控制方法及一种手术机器人
WO2017125994A1 (ja) * 2016-01-18 2017-07-27 オリンパス株式会社 医療用マニピュレータシステム、及びその作動方法
CN111856942A (zh) * 2020-08-03 2020-10-30 天津理工大学 一种远程介入手术控制系统的时滞与误差的改进方法
CN112057172A (zh) * 2020-09-10 2020-12-11 苏州大学 一种微创手术机器人

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP5776577B2 (ja) * 2012-02-14 2015-09-09 トヨタ自動車株式会社 マスタスレーブ制御方法及び制御装置並びにシステム
CN104647331B (zh) * 2015-03-23 2017-07-21 常州米泽智能装备科技有限公司 一种主从随动示教工业机器人系统
CN108248681A (zh) * 2018-01-16 2018-07-06 广东电网有限责任公司电力科学研究院 一种四轮独立驱动转向机器人的主从控制系统
CN108638015B (zh) * 2018-05-16 2021-10-29 南京邮电大学 基于量化通信的主从机械臂同步跟踪控制方法
CN110497411B (zh) * 2019-08-23 2020-11-24 华中科技大学 一种工业机器人协同运动控制方法
CN112417755A (zh) * 2020-11-10 2021-02-26 西安工程大学 一种主从式手术机器人轨迹预测控制方法
CN113814951B (zh) * 2021-08-16 2023-04-07 深拓科技（深圳）有限公司 一种主从机器人的控制方法、系统及存储介质

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH04372380A (ja) * 1991-06-17 1992-12-25 Hitachi Ltd マニピュレ−タの操作装置
WO2017125994A1 (ja) * 2016-01-18 2017-07-27 オリンパス株式会社 医療用マニピュレータシステム、及びその作動方法
CN106137400A (zh) * 2016-05-31 2016-11-23 微创（上海）医疗机器人有限公司 用于机械臂的控制系统、控制方法及一种手术机器人
CN106003034A (zh) * 2016-06-16 2016-10-12 深圳先进技术研究院 一种主从式机器人控制系统及控制方法
CN111856942A (zh) * 2020-08-03 2020-10-30 天津理工大学 一种远程介入手术控制系统的时滞与误差的改进方法
CN112057172A (zh) * 2020-09-10 2020-12-11 苏州大学 一种微创手术机器人

Also Published As

Publication number Publication date
CN115847371A (zh) 2023-03-28
CN115847371B (zh) 2023-12-12

Similar Documents

Publication Publication Date Title
US9317032B2 (en) Robot and method for operating a robot
CN107901041A (zh) 一种基于图像混合矩的机器人视觉伺服控制方法
CN106003034B (zh) 一种主从式机器人控制系统及控制方法
CN107116553B (zh) 一种机械臂的操作方法及装置
CN112053398B (zh) 物体抓取方法、装置、计算设备和存储介质
JP2017094438A (ja) 学習による教示作業の簡易化及び動作性能向上機能を備えたロボットシステム
WO2022166329A1 (zh) 机器人的人机协作控制方法、装置和机器人
JP2003211381A (ja) ロボット制御装置
CN107172359B (zh) 摄像头人脸跟踪系统及人脸跟踪方法
KR20180114837A (ko) 로봇의 이동 속도 제어 장치 및 방법
CN114347008A (zh) 基于工业机器人的无序抓取工件方法、装置及智能终端
CN115502979A (zh) 一种机械臂力矩主动柔顺精确控制方法及系统
WO2024021599A1 (zh) 一种主从手机器人的控制方法及装置
CN116214522B (zh) 基于意图识别的机械臂控制方法、系统及相关设备
CN110640735B (zh) 一种纠偏方法、纠偏装置及机器人
CN116494250A (zh) 基于速度补偿的机械臂控制方法、控制器、介质及系统
CN113568414A (zh) 一种机器人的运动控制方法、装置、机器人及存储介质
CN111590575B (zh) 一种机器人的控制系统及其方法
CN109483541B (zh) 一种基于分解速度规划算法的移动物体抓取方法
CN111113394B (zh) 机械手定位方法
CN113878578A (zh) 适用于复合型机器人的动态自适应定位方法和系统
CN112605990A (zh) 一种机器人视觉控制方法和系统
US20230381963A1 (en) Robot control method, computer-readable storage medium, and robot
US20220362934A1 (en) Control device, control method, and program
CN115816469B (zh) 一种基于机器视觉的云化plc控制物料分拣方法及系统

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 23844818

Country of ref document: EP

Kind code of ref document: A1