Granted Patent B2

US 12,734,673 · App. 18/995,293 · Granted Sep 15, 2026

Inventors: Yi Sun (Suzhou, CN); Pengfei Zhong (Suzhou, CN); Wei Luo (Suzhou, CN)

Assignee: Innolcon Medical Technology (Suzhou) Co., Ltd.

B25J3/04B25J9/1628

Abstract

Claims (78)

1 . A control method of a master-slave-manipulator robot, characterized in that at least one slave-manipulator motor is provided on a slave manipulator of the master-slave-manipulator robot, and the method is used for controlling motion of the slave-manipulator motor and comprises:

acquiring a mapped position of the slave-manipulator motor for a current cycle, wherein the mapped position is used for indicating a number of mapped rotational pulses, which is acquired by direct mapping based on three-dimensional coordinates of a master manipulator of the master-slave-manipulator robot and a mechanical structure of the master-slave-manipulator robot, the three-dimensional coordinates of the master manipulator being acquired at a preset acquisition cycle, and the current cycle being any acquisition cycle;

acquiring a starting motion parameter of the slave-manipulator motor for the current cycle, the starting motion parameter comprising a starting velocity V s and a starting acceleration A s ;

performing planning on a motion trajectory of the slave-manipulator motor for the current cycle based on the starting motion parameter;

acquiring a maximum planning distance P cm of the slave-manipulator motor for the current cycle;

determining, as a target distance, a value that is able to be reached preferentially among a mapped motion distance and the maximum planning distance P cm , from an actual position of the slave-manipulator motor for a previous cycle based on the starting motion parameter, wherein the mapped motion distance is a difference between the mapped position of the slave-manipulator motor for the current cycle and the actual position of the slave-manipulator motor for the previous cycle, the actual position being used for indicating a number of actual rotational pulses, and the previous cycle being a previous-one acquisition cycle of the current cycle; and

determining, as an actual position of the slave-manipulator motor for the current cycle, a sum of the actual position of the slave-manipulator motor for a previous cycle and the target distance.

2 . The method according to claim 1 , characterized by further comprising:

determining actual three-dimensional coordinates of the slave manipulator for the current cycle based on the actual position of the slave-manipulator motor for the current cycle;

detecting whether the actual three-dimensional coordinates are beyond maximal coordinates of the slave manipulator; and

if the actual three-dimensional coordinates are not beyond the maximal coordinates of the slave manipulator, controlling the slave-manipulator motor to move to the actual position for the current cycle.

3 . The method according to claim 2 , characterized by further comprising:

if the actual three-dimensional coordinates are beyond the maximal coordinates of the slave manipulator, acquiring a corrected position of the slave-manipulator motor for the current cycle based on the maximal coordinates of the slave manipulator; and

controlling the slave-manipulator motor to move to a corrected position for the current cycle.

4 . The method according to claim 2 , characterized by further comprising: before controlling the slave-manipulator motor to move,

if multi-axis motions of respective ones of the slave manipulators of the master-slave-manipulator robot are not synchronized for the current cycle, acquiring target positions of the slave-manipulator motors on the respective ones of the slave manipulators by using an inverse kinematic solution method based on target three-dimensional coordinates of the respective ones of the slave manipulators for the current cycle and the mechanical structure of the master-slave-manipulator robot;

acquiring target three-dimensional coordinates of the master manipulator based on the target position of the slave-manipulator motor on each of the slave manipulators;

if the target three-dimensional coordinates of the master manipulator are beyond maximal coordinates of the master manipulator, correct the three-dimensional coordinates of the master manipulator for the current cycle to the maximal coordinates of the master manipulator; and

reacquiring, based on the maximal coordinates of the master manipulator, the mapped position of the slave-manipulator motor for the current cycle until the actual position of the slave-manipulator motor for the current cycle is re-determined.

5 . The method according to claim 4 , characterized by further comprising:

if the target three-dimensional coordinates of the master manipulator are not beyond the maximal coordinates of the master manipulator, correcting the three-dimensional coordinates of the master manipulator for the current cycle to the target three-dimensional coordinates of the master manipulator; and

reacquiring, based on the target three-dimensional coordinates of the master manipulator, the mapped position of the slave-manipulator motor for the current cycle until the actual position of the slave-manipulator motor for the current cycle is re-determined.

6 . The method according to claim 1 , characterized in that the acquiring a starting motion parameter of the slave-manipulator motor for the current cycle comprises:

acquiring actual positions of the slave-manipulator motor for three acquisition cycles previous to the current cycle, respectively; and

determining the starting velocity V s and the starting acceleration A s based on the actual positions of the slave-manipulator motor for the three acquisition cycles previous to the current cycle.

7 . The method according to claim 6 , characterized in that the determining the starting velocity V s and the starting acceleration A s based on the actual positions of the slave-manipulator motor for the three acquisition cycles previous to the current cycle comprises:

determining a first average velocity of the slave-manipulator motor for the previous cycle based on a difference between a first actual position and a second actual position and a duration of the acquisition cycle, the first actual position being an actual position of the slave-manipulator motor for the previous cycle, and the second actual position being an actual position of the slave-manipulator motor for the previous-one acquisition cycle of the previous cycle;

determining a second average velocity of the slave-manipulator motor for the previous-one acquisition cycle of the previous cycle based on a difference between the second actual position and a third actual position and the duration of the acquisition cycle, the third actual position being an actual position of the slave-manipulator motor for a third acquisition cycle previous to the current cycle;

determining an average acceleration based on the second average velocity, the first average velocity and the duration of the acquisition cycle; and

determining the first average velocity as the starting velocity V s , and determining the average acceleration as the starting acceleration A s .

8 . The method according to claim 1 , characterized by further comprising: before performing planning on a motion trajectory of the slave-manipulator motor for the current cycle based on the starting motion parameter,

determining a first motion direction of the slave-manipulator motor for the current cycle based on a mapped position of the slave-manipulator motor for the current cycle and an actual position of the slave-manipulator motor for the previous cycle;

determining a second motion direction of the slave-manipulator motor for the previous cycle based on the actual position of the slave-manipulator motor for the previous cycle and the actual position of the slave-manipulator motor for a previous-one acquisition cycle of the previous cycle; and

detecting whether the first motion direction is the same as the second motion direction.

9 . The method according to claim 8 , characterized in that, when the first motion direction is the same as the second motion direction, the performing planning on a motion trajectory of the slave-manipulator motor for the current cycle based on the starting motion parameter comprises:

detecting whether the starting velocity V s is equal to the maximal velocity V m of the slave-manipulator motor, and

if the starting velocity V s is equal to the maximal velocity V m , performing planning on the motion trajectory of the slave-manipulator motor for the current cycle to move uniformly motion at the maximal velocity V m until ending of the current cycle.

10 . The method according to claim 9 , characterized by further comprising:

if the starting velocity V s is less than the maximal velocity V m , detecting whether the starting acceleration A s is equal to the maximal acceleration A m ;

if the starting acceleration A s is equal to the maximal acceleration A m , determining a first duration required to accelerate uniformly to the maximal velocity V m from the starting velocity V s at the maximal acceleration A m ; and

if the first duration is less than a duration of the current cycle, performing planning on the motion trajectory of the slave-manipulator motor for the current cycle to first accelerate uniformly to the maximal velocity V m at the maximal acceleration A m , and then to move uniformly at the maximal velocity V m for the remaining duration of the current cycle until the ending of the current cycle.

11 . The method according to claim 10 , characterized by further comprising:

if the first duration is greater than the duration of the current cycle, performing planning on the motion trajectory of the slave-manipulator motor for the current cycle to accelerate uniformly from the starting velocity V s at the maximal acceleration A m until the ending of the current cycle.

12 . The method according to claim 10 , characterized by further comprising:

if the starting acceleration A s is less than the maximal acceleration A m , determining a second duration required to accelerate to the maximal acceleration A m from the starting acceleration A s at a first jerk;

if the second duration is less than the duration of the current cycle, acquiring an intermediate velocity at the ending of the second duration;

if the intermediate velocity is less than the maximal velocity V m , determining a third duration required to accelerate uniformly to the maximal velocity V m from the intermediate velocity at the maximal acceleration A m ; and

if a sum of the second duration and the third duration is less than the duration of the current cycle, performing planning on the motion trajectory of the slave-manipulator motor for the current cycle to first accelerate to the maximal acceleration A m at the first jerk, then to accelerate uniformly to the maximal velocity V m at the maximal acceleration A m , and finally to move uniformly at the maximal velocity V m for the remaining duration of the current cycle until the ending of the current cycle.

13 . The method according to claim 12 , characterized by further comprising:

if the sum of the second duration and the third duration is greater than the duration of the current cycle, performing planning on the motion trajectory of the slave-manipulator motor for the current cycle to first accelerate to the maximal acceleration A m from the starting acceleration A s , and then to accelerate uniformly at the maximal acceleration A m for the remaining duration of the current cycle until the ending of the current cycle.

14 . The method according to claim 12 , characterized by further comprising:

if the intermediate velocity is greater than the maximal velocity V m , performing planning on the motion trajectory of the slave-manipulator motor for the current cycle to perform a uniform jerk to the maximal velocity V m from the starting velocity V s at the first jerk, and then to move uniformly at the maximal velocity V m for the remaining duration of the current cycle until the ending of the current cycle.

15 . The method according to claim 12 , characterized by further comprising:

if the second duration is greater than the duration of the current cycle, determining an end velocity to which a uniform jerk is performed from the starting velocity V s at the first jerk until the ending of the current cycle; and

if the end velocity is less than the maximal velocity V m , performing planning on the motion trajectory of the slave-manipulator motor for the current cycle to perform a uniform jerk from the starting velocity V s at the first jerk until the ending of the current cycle.

16 . The method according to claim 15 , characterized by further comprising:

if the end velocity is greater than the maximal velocity V m , performing planning on the motion trajectory of the slave-manipulator motor for the current cycle to perform an uniform jerk to the maximal velocity V m from the starting velocity V s at the first jerk, and then to move uniformly at the maximal velocity V m for the remaining duration of the current cycle until the ending of the current cycle.

17 . The method according to claim 9 , characterized in that the acquiring a maximum planning distance P cm of the slave-manipulator motor for the current cycle comprises:

determining, as the maximum planning distance P cm of the slave-manipulator motor for the current cycle, a total motion distance traveled by the slave-manipulator motor for the current cycle.

18 . The method according to claim 8 , characterized in that, when the first motion direction is opposite to the second motion direction, said the performing planning on a motion trajectory of the slave-manipulator motor for the current cycle based on the starting motion parameter comprises:

detecting, based on the starting motion parameter and a second jerk, whether the slave-manipulator motor is able to decelerate to zero from the starting velocity V s for the current cycle;

if the slave-manipulator motor is able to decelerate to zero from the starting velocity V s for the current cycle, acquiring an intermediate position and an intermediate acceleration of the slave-manipulator motor when the slave-manipulator motor decelerates to zero from the starting velocity V s ; and

performing planning on the motion trajectory of the slave-manipulator motor for the remaining duration of the current cycle based on the motion parameter of the slave-manipulator motor at the intermediate position and the remaining duration of the current cycle, wherein the slave-manipulator motor at the intermediate position has a starting velocity of zero and a starting acceleration as the intermediate acceleration.

19 . The method according to claim 18 , characterized by further comprising:

if the slave-manipulator motor is not able to decelerate to zero from the starting velocity V s for the current cycle, detecting whether the slave-manipulator motor is able to reach the maximum acceleration in a deceleration section for the current cycle; and

if the slave-manipulator motor is able to reach the maximum acceleration in the deceleration section for the current cycle, performing planning on the motion trajectory of the slave-manipulator motor for the current cycle to first decelerate to the maximum acceleration in the deceleration section at the second jerk, and then to move by a uniform deceleration for the remaining duration of the current cycle until the ending of the current cycle.

20 . The method according to claim 19 , characterized by further comprising:

if the slave-manipulator motor is not able to reach the maximum acceleration in the deceleration section for the current cycle, performing planning on the motion trajectory of the slave-manipulator motor for the current cycle to move by a deceleration at the second jerk until the ending of the current cycle.

21 . The method according to claim 18 , characterized in that the acquiring a maximum planning distance P cm of the slave-manipulator motor for the current cycle comprises:

acquiring an end position of the slave-manipulator motor at the ending of the current cycle; and

determining, as the maximum planning distance P cm of the slave-manipulator motor for the current cycle, a difference between the end position and the actual position of the slave-manipulator motor for the previous cycle.

22 . A control device of a master-slave-manipulator robot, characterized in that at least one slave-manipulator motor is provided on a slave manipulator of the master-slave-manipulator robot, the device is configured to control motion of the slave-manipulator motor and comprises:

a mapping position acquisition module configured for acquiring a mapping position of the slave-manipulator motor for a current cycle, wherein the mapping position is used for indicating a number of mapped rotational pulses, which is acquired by direct mapping based on three-dimensional coordinates of the master manipulator and a mechanical structure of the master-slave-manipulator robot, the three-dimensional coordinates of the master manipulator being acquired at a preset acquisition cycle, and the current cycle being any acquisition cycle;

a starting motion parameter acquisition module configured for acquiring a starting motion parameter of the slave-manipulator motor for the current cycle, the starting motion parameter comprising a starting velocity V s and a starting acceleration A s ;

a motion planning module configured for performing planning on a motion trajectory of the slave-manipulator motor for the current cycle based on the starting motion parameter;

a maximum planning distance acquisition module configured for acquiring a maximum planning distance P cm of the slave-manipulator motor for the current cycle;

a target distance determination module configured for determining, as a target distance, a value that is able to be reached preferentially among a mapped motion distance and the maximum planning distance P cm , from an actual position of the slave-manipulator motor for a previous cycle based on the starting motion parameter, wherein the mapped motion distance is a difference between the mapped position of the slave-manipulator motor for the current cycle and the actual position of the slave-manipulator motor for the previous cycle, the actual position being used for indicating a number of actual rotational pulses, and the previous cycle being a previous-one acquisition cycle of the current cycle; and

an actual position determination module configured for determining, as an actual position of the slave-manipulator motor for the current cycle, a sum of the actual position of the slave-manipulator motor for a previous cycle and the target distance.

Assignments (1)

Priority Claims (1)

Continuity (1)

References Cited (14)