CN115703227A - Robot control method, robot, and computer-readable storage medium - Google Patents
Robot control method, robot, and computer-readable storage medium Download PDFInfo
- Publication number
- CN115703227A CN115703227A CN202110884890.7A CN202110884890A CN115703227A CN 115703227 A CN115703227 A CN 115703227A CN 202110884890 A CN202110884890 A CN 202110884890A CN 115703227 A CN115703227 A CN 115703227A
- Authority
- CN
- China
- Prior art keywords
- tail end
- mechanical arm
- information
- instrument
- robot
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Abstract
The present application relates to a control method of a robot, and a computer-readable storage medium. According to the method, the position information of the telecentric motionless point is determined in advance, so that the tail end of the executive instrument is controlled to reach the target position according to the displacement increment provided by the main manipulator in the motion control of the slave mechanical arm by taking the telecentric motionless point to keep a fixed position as a constraint condition, active RCM motion constraint is realized, and the problems of complex structural design and large space occupancy rate of the slave mechanical arm of the robot caused by the fact that the RCM motion constraint in the related technology depends on a parallelogram structure are solved. According to the embodiment, the dependence of RCM motion constraint on the structure of the driven mechanical arm is avoided, the structural design difficulty of the driven mechanical arm can be reduced, and the design and manufacturing cost is obviously reduced.
Description
Technical Field
The present application relates to the field of machine control, and more particularly, to a robot control method, a robot, and a computer-readable storage medium.
Background
When a micro slit opening is formed in the surface of an object and precision machining is performed on the inside of the object through the slit opening, in order to avoid damage to the slit opening, the robot needs to move around the slit opening, which is called a Remote Center of Motion (RCM). In the existing robot, RCM motion constraint is realized by depending on a parallelogram structure, but the RCM motion constraint causes the structural design of a driven mechanical arm of the robot to be complex and the space occupancy rate to be large, and further causes the design and manufacturing cost to be increased.
Aiming at the problem that the RCM motion constraint in the related art depends on a parallelogram structure, so that the structural design of a driven mechanical arm of a robot is complex, an effective solution is not provided at present.
Disclosure of Invention
In the present embodiment, a control method of a robot, a robot and a computer readable storage medium are provided to solve the problem in the related art that the RCM motion constraint depends on a parallelogram structure, resulting in a complicated structural design of a driven mechanical arm of the robot.
In a first aspect, in this embodiment, a control method for a robot is provided, where the robot includes a slave manipulator, an execution link is fixedly connected to a manipulator end of the slave manipulator, and an execution instrument is disposed at the manipulator end, and the control method for the robot includes:
step 1, determining a point on the execution connecting rod as a telecentric motionless point of the driven mechanical arm, and determining the position information of the telecentric motionless point;
step 2, determining the position information of the tail end of the execution instrument at the current position, and determining the position information of the tail end of the execution instrument at the target position based on the displacement increment of the tail end of the execution instrument provided by the main manipulator and the position information in the position information of the tail end of the execution instrument at the current position;
and 3, determining a pose conversion relation of the tail end of the mechanical arm moving from the current position to the target position by taking the telecentric fixed point holding fixed position as constraint on the basis of pose information of the tail end of the execution instrument at the current position and position information of the tail end of the execution instrument at the target position, resolving the pose conversion relation to obtain joint angles of all joints of the driven mechanical arm on the basis of inverse kinematics, and controlling the driven mechanical arm on the basis of the joint angles of all the joints of the driven mechanical arm.
In some of these embodiments, determining the position information of the performing instrument tip at the target position based on the displacement increment provided by the main manipulator and the position information of the performing instrument tip in the pose information of the current position comprises:
acquiring displacement increment provided by the main manipulator;
sequentially decomposing the displacement increment into a plurality of target displacement increments according to the motion sequence;
determining position information of a plurality of target positions which are sequentially arranged according to a motion sequence based on each target displacement increment and the position information of the tail end of the execution instrument at the current position;
wherein, for each target position, the step 3 is executed in turn according to the motion sequence.
In some of these embodiments, sequentially decomposing the displacement increment into a plurality of target displacement increments in the motion order comprises:
decomposing the displacement increment into a plurality of target displacement increments according to a preset step length or a preset interpolation position quantity under the condition that the displacement increment is larger than a second preset threshold value;
and taking the displacement increment as the target displacement increment when the displacement increment is smaller than the second preset threshold.
In some of these embodiments, after step 3, the method further comprises:
determining target pose information of the executing instrument tail end at a target position based on the posture information provided by the main operator and the position information of the executing instrument tail end at the target position;
calculating joint angles of all joints of the execution instrument according to the initial pose information and the target pose information of the tail end of the execution instrument on the basis of inverse kinematics, and controlling the execution instrument on the basis of the joint angles of all the joints of the execution instrument.
In some of these embodiments, the position of the implement tip is located on the extension of the implement linkage; based on the pose information of the tail end of the execution instrument at the current position and the position information of the tail end of the execution instrument at the target position, and taking the telecentric fixed point as a constraint, determining the pose conversion relation of the tail end of the mechanical arm moving from the current position to the target position comprises the following steps:
determining posture information of the tail end of the mechanical arm at the target position based on the target position of the tail end of the execution instrument and the posture of the straight line passing through the telecentric fixed point;
determining position information of the end of the robotic arm at a target location on the straight line based on a length of the end of the performance instrument to the end of the robotic arm;
and determining the posture conversion relation of the tail end of the mechanical arm moving from the current position to the target position according to the posture information of the tail end of the mechanical arm at the current position, the posture information of the tail end of the mechanical arm at the target position and the position information of the tail end of the mechanical arm at the target position.
In some of these embodiments, the pose of the end of the performing instrument relative to the end of the robotic arm is fixed while the slave robotic arm is controlled based on the joint angles of the joints of the slave robotic arm.
In some of the embodiments described herein, the first and second,
the telecentric motionless point is arranged at a preset position on the execution connecting rod, and the position information of the telecentric motionless point is determined based on the preset position and the pose information of the tail end of the mechanical arm at the current position; or
The position information of the telecentric motionless point is determined based on the measurement.
In some of these embodiments, where the position information of the telecentric motionless point is determined based on measurements, the method further comprises:
determining pose information of the tail end of the mechanical arm at the current position;
measuring the length from the tail end of the mechanical arm to a preset position on the execution connecting rod;
and determining the position information of the preset position based on the length and the pose information of the tail end of the mechanical arm at the current position, and taking the position information of the preset position as the position information of the telecentric motionless point.
In a second aspect, the present embodiment provides a robot, including a main manipulator, a slave manipulator, and a control device, wherein an execution link is fixedly connected to a manipulator end of the slave manipulator, an execution instrument is disposed at the manipulator end, and the control device includes a memory and a processor, the memory stores a computer program, and the processor is configured to execute the computer program to execute the steps of the control method of the robot according to the first aspect.
In a third aspect, in the present embodiment, there is provided a computer-readable storage medium having stored thereon a computer program which, when executed by a processor, implements the steps of the control method of the robot according to the first aspect.
Compared with the related art, the control method of the robot, and the computer-readable storage medium provided in the present embodiment first determine a point on the execution link as a telecentric motionless point of the driven mechanical arm, and determine position information of the telecentric motionless point; then, determining the pose information of the tail end of the execution instrument at the current position, and determining the position information of the tail end of the execution instrument at the target position based on the displacement increment of the tail end of the execution instrument provided by the main manipulator and the position information of the tail end of the execution instrument in the pose information of the current position; and finally, based on the pose information of the tail end of the execution instrument at the current position and the position information of the tail end of the execution instrument at the target position, keeping the fixed position of a telecentric fixed point as constraint, determining the pose conversion relation of the tail end of the mechanical arm moving from the current position to the target position, resolving the pose conversion relation based on inverse kinematics to obtain the joint angle of each joint of the driven mechanical arm, and controlling the driven mechanical arm based on the joint angle of each joint of the driven mechanical arm.
The details of one or more embodiments of the application are set forth in the accompanying drawings and the description below to provide a more thorough understanding of the application.
Drawings
The accompanying drawings, which are included to provide a further understanding of the application and are incorporated in and constitute a part of this application, illustrate embodiment(s) of the application and together with the description serve to explain the application and not to limit the application. In the drawings:
fig. 1 is a schematic structural diagram of a robot provided in an embodiment of the present application.
Fig. 2 is a schematic structural diagram of the control device 30 according to the embodiment of the present application.
Fig. 3 is a flowchart of a control method of a robot according to an embodiment of the present application.
Fig. 4 is a first flowchart of a robot control method according to a preferred embodiment of the present application.
FIG. 5 is a schematic illustration of interpolation and motion of the trajectory of the end of the robotic arm in accordance with a preferred embodiment of the present application.
Fig. 6 is a second flowchart of a robot control method according to a preferred embodiment of the present application.
Fig. 7 is a diagram showing simulation results of the robot arm tip trajectory and the manipulator tip trajectory in the robot control method according to the embodiment of the present application.
In the drawings:
TCP, end of mechanical arm;
RCM, telecentric motionless point;
10. a main manipulator;
20. a driven mechanical arm; 201. an execution connecting rod; 202. an implement; 2021. an implement tip;
30. a control device; 301. a memory; 3011. an operating system; 3012. a network communication module; 3013. a presentation module; 3014. an input processing module; 3015. a computer program; 30151. a first determination module; 30152. a second determination module; 30153. a position control module; 30154. an attitude control module; 302. a processor; 303. a network interface; 304. a user interface;
40. the surface of the object.
Detailed Description
For a clearer understanding of the objects, technical solutions and advantages of the present application, reference is made to the following description and accompanying drawings.
Unless defined otherwise, technical or scientific terms used herein shall have the same general meaning as commonly understood by one of ordinary skill in the art to which this application belongs. The use of the terms "a" and "an" and "the" and similar referents in the context of describing the invention (including a reference to the context of the specification and claims) are to be construed to cover both the singular and the plural, as well as the singular and plural. The terms "comprises," "comprising," "has," "having," and any variations thereof, as referred to in this application, are intended to cover non-exclusive inclusions; for example, a process, method, and system, article, or apparatus that comprises a list of steps or modules (elements) is not limited to the listed steps or modules, but may include other steps or modules (elements) not listed or inherent to such process, method, article, or apparatus. Reference throughout this application to "connected," "coupled," and the like is not limited to physical or mechanical connections, but may include electrical connections, whether direct or indirect. Reference to "a plurality" in this application means two or more. "and/or" describes an association relationship of associated objects, meaning that three relationships may exist, for example, "A and/or B" may mean: a exists alone, A and B exist simultaneously, and B exists alone. In general, the character "/" indicates a relationship in which the objects associated before and after are an "or". The terms "first," "second," "third," and the like in this application are used for distinguishing between similar items and not necessarily for describing a particular sequential or chronological order.
Fig. 1 is a schematic structural diagram of a robot according to an embodiment of the present invention, and as shown in fig. 1, the robot includes a master manipulator 10, a slave manipulator 20, and a control device 30. The master manipulator 10 and the slave manipulator 20 are electrically connected to the control device 30, respectively.
The master manipulator 10 may be the same as the slave robot arm 20, or may be a different master manipulator from the slave robot arm 20. When a signal generated by the master manipulator 10 is transmitted to the control device 30 and is calculated by the control device 30 to generate joint angle signals of the respective joints of the slave manipulator 20, the slave manipulator 20 performs motion control based on the joint angle signals of the respective joints generated by the control device 30.
Referring to the slave robot arm 20 shown in fig. 1, there may be one or more slave robot arms 20 and one or more master manipulators 10 in a robot. Preferably, a robot may have two or more slave robot arms 20, each slave robot arm 20 has an actuating link 201 with a certain length fixedly connected to a distal end of the slave robot arm 20, and an actuating device 202 for performing a precise operation is provided at a distal end of the actuating link 201. The execution devices 202 of different slave mechanical arms 20 in the robot can be of different types, so that the times of interrupting the control process and replacing the execution devices 202 can be reduced in the control process of one robot, and the corresponding slave mechanical arms 20 are only needed to be selected to perform precise operation in different operation tasks. The slave robotic arm 20 may be configured as a tandem robotic arm and may further include at least one parallel platform.
Considering the scene of single-person control, the number of the main operators 10 is usually one or two, and heterogeneous main operators are adopted to improve the operation experience. In the case of two main hands 10, each main hand 10 corresponds to the left and right hands of the human hand, respectively. In the case where the number of slave robot arms 20 is the same as the number of master robot arms 10, each master robot arm 10 can control one slave robot arm 20 accordingly. In the case where the number of the slave arms 20 is greater than the number of the master manipulators 10, each master manipulator 10 may select one of the slave arms 20 to control, and the slave arms 20 that are not selected are not controlled by the master manipulator 10.
In this embodiment, the position of the slave manipulator 20 is controlled by a master-slave control method based on displacement increments, that is, the displacement increment of the end of the master manipulator 10 is mapped to the displacement increment of the end of the execution instrument 202 of the slave manipulator 20, where the multiple relationship of the displacement increment mapping from the master manipulator 10 to the slave manipulator 20 is adjustable, and the multiple relationship may be greater than 1 or less than 1. In the case where the multiple relation is greater than 1, that is, the displacement increment of the master manipulator 10 is amplified and mapped to the slave manipulator 20, in this case, the slave manipulator 20 can be controlled to move rapidly. In the case where the multiple relation is less than 1, that is, the displacement increment of the master manipulator 10 is reduced and mapped to the slave manipulator 20, it is possible to control the slave manipulator 20 to perform a highly accurate movement and remove the shake.
In some of these embodiments, the displacement increment control enables position control of the slave robotic arm 20, and the slave robotic arm 20 is a tandem robotic arm. Based on this, the joint angles of the joints of the slave manipulator 20 can be efficiently obtained by inverse kinematics solution using the displacement increment in combination with the advantage of high inverse kinematics solution efficiency of the tandem manipulator.
The posture of the actuator of the slave manipulator 20 can be obtained by inverse kinematics calculation based on the posture information (i.e., joint angle) of the master manipulator 10 and the target position of the end of the actuator.
Fig. 2 is a schematic structural diagram of a control device 30 according to an embodiment of the present disclosure, and as shown in fig. 2, the control device 30 according to the present embodiment includes a memory 301 and at least one processor 302.
The memory 301 may be removable, non-removable, or a combination thereof. Exemplary hardware devices include solid state memory, hard disk drives, optical disk drives, and the like. Memory 301 optionally includes one or more storage devices physically located remote from processor 302. The memory 301 may include volatile memory or nonvolatile memory, and may also include both volatile and nonvolatile memory. The nonvolatile Memory may be a Read Only Memory (ROM), and the volatile Memory may be a Random Access Memory (RAM). The memory 301 described in embodiments herein is intended to comprise any suitable type of memory.
In some embodiments, memory 301 is capable of storing data, examples of which include programs, modules, and data structures, or a subset or superset thereof, to support various operations, as exemplified below.
Operating system 3011, which includes system programs for handling various basic system services and performing hardware related tasks, such as framework layers, core library layers, driver layers, etc., is used for implementing various basic services and for handling hardware based tasks.
Network communication module 3012 for reaching other computing devices via one or more (wired or wireless) network interfaces 420, exemplary network interfaces 303 include: bluetooth, wireless fidelity (WiFi), and Universal Serial Bus (USB), among others.
A presentation module 3013 for enabling presentation of information (e.g., a user interface for operating peripherals and displaying content and information) via one or more output devices (e.g., a display screen, speakers, etc.) associated with the user interface 304.
An input processing module 3014 for detecting one or more user inputs or interactions from one of the one or more input devices and translating the detected inputs or interactions.
Computer program 3015, which may include the following software modules:
the first determining module 30151 is configured to determine a point on the execution link as a telecentric motionless point of the driven mechanical arm, and determine position information of the telecentric motionless point.
A second determining module 30152, configured to determine pose information of the tip of the performing instrument at the current position, and determine position information of the tip of the performing instrument at the target position based on the displacement increment of the tip of the performing instrument provided by the main operator and the position information of the tip of the performing instrument in the pose information of the current position.
And the position control module 30153 is configured to determine, based on pose information of the end of the execution instrument at the current position and position information of the end of the execution instrument at the target position, a pose conversion relationship in which the end of the mechanical arm moves from the current position to the target position with the telecentric motionless point being kept at the fixed position as a constraint, obtain joint angles of joints of the driven mechanical arm through calculation from the pose conversion relationship based on inverse kinematics, and control the driven mechanical arm based on the joint angles of the joints of the driven mechanical arm.
In some of these embodiments, the computer program 3015 may also include the following software modules:
a posture control module 30154 configured to determine target posture information of the end of the execution instrument at the target position based on the posture information provided by the main operator and the position information of the end of the execution instrument at the target position; and calculating the joint angle of each joint of the execution instrument according to the initial pose information and the target pose information of the tail end of the execution instrument on the basis of inverse kinematics, and controlling the execution instrument on the basis of the joint angle of each joint of the execution instrument.
The modules described above are logical and thus may be combined or further split according to the functionality implemented. The functions of the respective modules will be explained below.
The Processor 302 may be an integrated circuit chip having Signal processing capabilities, such as a general purpose Processor, a Digital Signal Processor (DSP), or other programmable logic device, discrete gate or transistor logic device, discrete hardware components, or the like, wherein the general purpose Processor may be a microprocessor or any conventional Processor, or the like.
The processor 302 is arranged to run the above-mentioned computer program 3015 to execute the control method of the robot provided by the present embodiment.
The control device 30 may also include at least one network interface 303 and at least one user interface 304. The various components in control device 30 may be coupled together by a bus system 305. It will be appreciated that the bus system 305 is used to enable connected communication between these components. The bus system 305 includes a power bus, a control bus, and a status signal bus in addition to a data bus. For clarity of illustration, however, the various buses are labeled as bus system 305 in FIG. 2.
The user interface 304 includes one or more output devices (not shown) including one or more speakers and/or one or more visual display screens that enable the presentation of media content. The user interface 304 also includes one or more input devices (not shown), including user interface components to facilitate user input, such as a keyboard, mouse, microphone, touch screen display, camera, other input buttons and controls.
The embodiment of the application provides a control method of a robot, which can at least solve the problem that RCM motion constraint depends on a parallelogram structure, so that the structural design of a driven mechanical arm of the robot is complex.
The following describes a control method of a robot provided in an embodiment of the present application, with reference to exemplary applications and implementations of the robot and the control apparatus provided in the embodiment of the present application.
Fig. 3 is a flowchart of a control method of a robot according to an embodiment of the present application, and as shown in fig. 3, the control method of the robot includes the steps of:
step S301, a point is determined on the actuator link as a telecentric motionless point of the driven robot arm, and position information of the telecentric motionless point is determined.
Step S302, determining the pose information of the tail end of the execution instrument at the current position, and determining the position information of the tail end of the execution instrument at the target position based on the displacement increment of the tail end of the execution instrument provided by the main manipulator and the position information of the tail end of the execution instrument in the pose information of the current position.
And step S303, based on the pose information of the tail end of the execution instrument at the current position and the position information of the tail end of the execution instrument at the target position, taking a telecentric fixed point to keep a fixed position as constraint, determining the pose conversion relation of the tail end of the mechanical arm moving from the current position to the target position, resolving the pose conversion relation based on inverse kinematics to obtain the joint angle of each joint of the driven mechanical arm, and controlling the driven mechanical arm based on the joint angle of each joint of the driven mechanical arm.
Through the steps, the position information of the telecentric motionless point is determined in advance, so that the tail end of the execution instrument is controlled to reach the target position according to the displacement increment provided by the main manipulator and by taking the telecentric motionless point to keep a fixed position as a constraint condition in the motion control of the driven mechanical arm, the active RCM motion constraint is realized, and the problems of complex structural design and large space occupancy rate of the driven mechanical arm of the robot caused by the fact that the RCM motion constraint in the related technology depends on a parallelogram structure are solved. According to the embodiment, the dependence of RCM motion constraint on the structure of the driven mechanical arm is avoided, the structural design difficulty of the driven mechanical arm can be reduced, and the design and manufacturing cost is obviously reduced.
Unless otherwise specified, the positional information in the embodiments of the present application refers to positional information (i.e., coordinate information) in a same coordinate system, and this coordinate system is generally referred to as a global coordinate system, a world coordinate system, or a mechanical coordinate system.
When the robot control method of the embodiment is adopted, the robot control method is firstly performed with positioning, that is, the position of the driven mechanical arm is placed in an application scene, for example, an execution connecting rod of the driven mechanical arm is inserted into a slit opening of an object, and the pose information of the tail end of an execution instrument of the driven mechanical arm or the pose information of the tail end of the mechanical arm at the current position at the moment can be recorded.
The position information of the telecentric motionless point in the step S301 may be determined in various ways.
For example, a telecentric motionless point is disposed at a predetermined position on the actuator linkage. In this case, length information of a connection point on the actuator link from the actuator may be determined from a preset position, and position information of a telecentric motionless point may be calculated based on posture information of the actuator and position information of the tip of the actuator at the current position. Or the length information of the executing connecting rod from the tail end of the mechanical arm can be determined through a preset position, and the position information of the telecentric motionless point is calculated and obtained based on the attitude information of the tail end of the mechanical arm and the position information of the tail end of the mechanical arm at the current position. By the mode, the problem that telecentric fixed points are not easy to obtain is solved, extra positioning operation is not needed, and the method is simple and easy to implement.
As another example, the telecentric motionless point described above can be determined based on measurements. Ways to measure telecentric motionless points include self-measurements and machine vision-based measurements.
The measurement based on machine vision obtains the intersection point of the executing connecting rod and the slit opening after identifying the slit opening and the executing connecting rod through target detection and the conversion relation between the image coordinate system and the mechanical coordinate system, and further obtains the position information of the telecentric motionless point through a coordinate system conversion mode. By the mode, the problem that the telecentric motionless point is not easy to obtain is solved, the position of the telecentric motionless point in each control can be changed according to scene needs, the problem of insufficient flexibility caused by the fact that the telecentric motionless point is fixed at the preset position on the execution connecting rod is solved, and the flexibility of selecting the telecentric motionless point is improved.
The self-measurement may be, for example: through the twice swing positions of the driven mechanical arm, the execution connecting rods in the twice swing positions are intersected at one point, the intersection point is a telecentric motionless point, or a straight line which is commonly perpendicular to the execution connecting rods in the twice swing positions is found, and the midpoint of a line segment formed by the intersection points of the straight line and the execution connecting rods is the telecentric motionless point. Another self-measuring method may be that the motion of the driven mechanical arm is controlled, the tail end of the execution instrument is moved to the opening of the slit, and the position information of the tail end of the execution instrument is recorded, that is, the position information of the telecentric fixed point. By the mode, the problem that the telecentric motionless point is not easy to obtain is solved, the position of the telecentric motionless point in each control can be changed according to scene needs, and the problem of insufficient flexibility caused by the fact that the telecentric motionless point is fixed at the preset position on the execution connecting rod is solved. Moreover, the mode is realized through twice positioning without additional visual equipment, and the realization cost is low, and the method is simple and easy to implement.
In addition, the embodiment also provides a mode for obtaining the position information of the telecentric fixed point by self-measurement: determining pose information of the tail end of the mechanical arm at the current position; measuring the length from the tail end of the mechanical arm to a preset position on the execution connecting rod; and determining the position information of the preset position based on the length and the pose information of the tail end of the mechanical arm at the current position, and taking the position information of the preset position as the position information of the telecentric motionless point. The method is similar to the acquisition method that the telecentric fixed point is arranged at the preset position of the execution connecting rod, namely the position of the telecentric fixed point, and compared with the acquisition method, the length information is obtained by measurement in the method, so that the preset position on the execution connecting rod does not need to be just arranged at the opening of the slit in the arranging stage, and the position of the telecentric fixed point can be more flexibly arranged.
It should be particularly noted that although the description is made such that the telecentric motionless point is set at the preset position on the actuator link, the expression is intended to explain that the position information of the telecentric motionless point can be determined based on the setting when the position information of the telecentric motionless point is determined, and does not mean that the preset position on the actuator link is always maintained at the fixed position during the control of the driven robot arm, for example, the telecentric motionless point can be moved on the actuator link during the control of the driven robot arm, as it appears that the depth of the actuator link from the slit opening into the object is different, but the actuator link always passes through the position of the telecentric motionless point determined in the present embodiment.
In step S303, the posture conversion relationship is expressed in a matrix form. In the embodiment, the posture conversion relationship between two adjacent joints of the driven mechanical arm is characterized by using robot link parameters (DH parameters) or improved DH parameters. Taking a DH parameter as an example, a joint coordinate system is established on each joint, and two adjacent joint coordinate systems or the 1 st joint coordinate system and a mechanical coordinate system can be overlapped with each other by rotating θ and translating d around the Z axis and then rotating α and translating a around the X axis, where θ, d, α and a are the DH parameters.
The inverse kinematics solution is to calculate the joint angle of each joint of the mechanical arm according to the position information of the joint end and the posture conversion relation. Inverse kinematics solutions are based on kinematic equations.
Taking a prio (UR) robot with 6 degrees of freedom of a driven mechanical arm as an example, the kinematic equation is as follows:
taking an executing instrument with 4 degrees of freedom as an example, the kinematic equation is as follows:
wherein the content of the first and second substances,
indicating the position and attitude of the UR robot arm tip (with TCP as its identifier) relative to the UR base (i.e. the machine coordinate system, with 0 as its identifier),
indicating the position and attitude of the 1 st joint of the UR robot arm (i.e. the joint closest to the UR base, identified by 1) relative to the UR base, and so on. Because the executing instrument is fixedly connected at the tail end of the mechanical arm (namely TCP) through the executing connecting rod,
indicating the position and attitude of the Tool Center Point (TCP for short) of the instrument end relative to the UR end. Then the
Indicating the position and pose of the end of the implement relative to the UR base.
In order to simplify the operation, the position of the tail end of the execution instrument can be placed on the extension line of the execution connecting rod in the positioning stage. Then, in step S303, the pose conversion relationship of the end of the robot arm moving from the current position to the target position may be obtained by: determining the posture information of the tail end of the mechanical arm at the target position based on the target position of the tail end of the execution instrument and the posture of the straight line passing through the telecentric fixed point; determining position information of the end of the mechanical arm at the target position on a straight line based on the length from the end of the execution instrument to the end of the mechanical arm; and determining the posture conversion relation of the tail end of the mechanical arm moving from the current position to the target position according to the posture information of the tail end of the mechanical arm at the current position, the posture information of the tail end of the mechanical arm at the target position and the position information of the tail end of the mechanical arm at the target position. The constraint that the telecentric motionless point keeps the fixed position means that a connecting line of the current position of the tail end of the mechanical arm and the current position of the tail end of the execution instrument passes through the telecentric motionless point, and a connecting line of the target position of the tail end of the mechanical arm and the target position of the tail end of the execution instrument also passes through the telecentric motionless point. In this way, it is achieved that the telecentric motionless point remains motionless at a fixed position at all times. Wherein, the attitude transformation relation can be obtained by an axis angle method.
If the position of the tail end of the execution instrument is not placed on the extension line of the execution connecting rod in the preoperative positioning stage, the position information of the tail end of the execution instrument at the current position can be projected to the straight line where the execution connecting rod is located, and the position information of the current projection point of the tail end of the execution instrument at the current position can be obtained; and projecting the position information of the tail end of the execution instrument at the target position to the straight line where the execution connecting rod is located to obtain the position information of the target projection point of the tail end of the execution instrument at the target position. Then, determining the posture information of the tail end of the mechanical arm at the target position based on the position of the target projection point passing through the tail end of the execution instrument and the posture of the straight line passing through the telecentric fixed point; determining position information of the tail end of the mechanical arm at a target position on a straight line based on the length from the target projection point of the tail end of the execution instrument to the tail end of the mechanical arm; and determining the posture conversion relation of the tail end of the mechanical arm moving from the current position to the target position according to the posture information of the tail end of the mechanical arm at the current position, the posture information of the tail end of the mechanical arm at the target position and the position information of the tail end of the mechanical arm at the target position. The constraint that the telecentric fixed position is kept is that a connecting line of the current position of the tail end of the mechanical arm and the current projection point of the tail end of the execution instrument passes through the telecentric fixed point, and a connecting line of the target position of the tail end of the mechanical arm and the target projection point of the tail end of the execution instrument also passes through the telecentric fixed point. In this way, it is achieved that the telecentric motionless point remains motionless at a fixed position at all times.
When the displacement increment provided by the master manipulator is too large, the distance between the current position and the target position may be too far, so that the joint angle value of each joint obtained by the slave manipulator through inverse kinematics calculation is too large, and the slave manipulator shakes and runs unstably. For this reason, smooth control of the slave robot is realized by interpolating the displacement increment of the slave robot end in the present embodiment.
The interpolation of the displacement increment can be the interpolation of the displacement increment provided by the main operator, and also can be the interpolation of the displacement increment of the tail end of the execution instrument.
For example, when interpolating the displacement increment provided by the master manipulator, the actual displacement increment of the master manipulator may be divided into a plurality of displacement increments, and the plurality of displacement increments may be sequentially provided to the control device or the slave manipulator in the order of the motions. The control device or the slave robot arm performs the steps S302 and S303 with the displacement increment provided by each master manipulator, thereby realizing smooth control of the slave robot arm. And each displacement increment of the main manipulator obtained by decomposition is not greater than a first preset threshold. Wherein, the first preset threshold value can be set to 0.1mm or other values.
For another example, when interpolating the displacement increment of the end of the executing instrument, the displacement increment provided by the main operator may be obtained first; sequentially decomposing the displacement increment into a plurality of target displacement increments according to the motion sequence; and determining position information of a plurality of target positions sequentially arranged in the movement order based on the displacement increments of the targets and the position information of the current position. Wherein, for each target position, the above step S303 is sequentially executed in the order of movement, thereby realizing smooth control of the slave manipulator.
Before the interpolation is specifically performed, it may be determined whether the displacement increment of the end of the execution instrument exceeds a second preset threshold (for example, the interpolation step length may be set to 0.1mm or another numerical value), and if the displacement increment of the end of the execution instrument does not exceed the interpolation step length, the pose conversion relationship determined according to the displacement increment may be used as a basis for determining each joint angle of the driven mechanical arm without performing interpolation processing. If the displacement increment of the tail end of the execution instrument exceeds the second preset threshold value, the displacement increment can be decomposed into a plurality of target displacement increments according to the preset interpolation step length or the preset interpolation position number. In the above manner, interpolation processing is adaptively performed according to the step length or the interpolation number by judging whether the displacement increment at the tail end of the execution instrument checks the second preset threshold, so that the calculation resource can be saved, and the calculation efficiency can be improved.
In step S303, in the process of controlling the slave robot arm based on the joint angle of each joint of the slave robot arm, the position and posture of the robot arm tip are changed at the same time, and the inversion motion and/or the advance and retreat motion of the actuator link occur with the change of the position and posture of the robot arm tip, but the actuator link always passes through the fixed position of the telecentric motionless point determined in the present embodiment. And in the present embodiment, the pose of the end of the performance instrument relative to the end of the robotic arm is fixed to reduce control complexity.
After step S303, the control method of the robot according to the present embodiment may further determine target pose information of the tip of the effector at the target position based on the pose information provided by the main manipulator and the position information of the tip of the effector at the target position; and resolving the joint angle of each joint of the execution instrument according to the initial pose information and the target pose information of the tail end of the execution instrument based on the inverse kinematics, and controlling the execution instrument based on the joint angle of each joint of the execution instrument. Through the mode, the position control of the tail end of the executing instrument is firstly carried out, then the attitude control of the executing instrument is carried out, the decoupling of the motion control of the driven mechanical arm and the motion control of the executing instrument is realized, namely the decoupling of the position control and the attitude control of the executing instrument is realized, and the control complexity is simplified.
Fig. 4 is a flowchart of a control method of a robot according to a preferred embodiment of the present application, and as shown in fig. 4, the control method of the robot according to the embodiment of the present application mainly includes two stages:
the first stage is as follows: after the initial positioning of the slave mechanical arm of the robot is finished, during master-slave control, the pose information of the tail end of a master manipulator is collected in real time and sent to the slave mechanical arm, the expected pose of the tail end of an executing instrument is obtained through a master-slave mapping algorithm, the expected pose is converted into the pose of the tail end of the mechanical arm, the motion control of the slave mechanical arm can be realized, the executing instrument is driven, and the executing instrument can reach the expected position under the condition that RCM constraint is met.
And a second stage: the executing instrument combines the current position information and the expected attitude information of the tail end of the executing instrument, the angle of each joint of the executing instrument is obtained through inverse kinematics calculation, the executing instrument is controlled to move to complete corresponding attitude adjustment, finally the executing instrument reaches the designated attitude transmitted by the main operating hand, and finally corresponding operation is completed.
Referring to fig. 6, still taking the UR robot as an example, with the above embodiment, after the slave mechanical arm is initially set, initial pose information of the end of the effector with respect to the UR base (i.e., the mechanical coordinate system) can be obtained. In a Cartesian space, the execution apparatus selects the number of reasonable interpolation points according to displacement increment information transmitted by a main manipulator and the length of the displacement increment, and interpolates the displacement increment. The posture conversion relation of the driven mechanical arm corresponding to the adjacent interpolation points is obtained by combining the slit opening position (namely the position of the RCM point) and the displacement interpolation point information, and the posture information of the interpolation point at the tail end of the execution instrument relative to the UR base can be formed on the basis of the initial posture information of the execution instrument. Bonding with
The kinematic relationship of the manipulator can convert the terminal pose of the execution instrument into the TCP pose of the UR terminal, the inverse kinematics of the manipulator is utilized to obtain the joint angle of the manipulator, the manipulator is controlled to move, the instrument is driven to reach the position of a focus under the condition that RCM constraint is met, and the first stage is completed, wherein the movement process is as shown in fig. 5.
And then, combining the current position information and the posture information transmitted by the main manipulator, obtaining the joint angle of the execution instrument through inverse kinematics of the instrument, moving to complete corresponding posture adjustment, finally enabling the execution instrument to reach the designated posture transmitted by the main manipulator, and finally completing corresponding operation, thereby completing the second stage.
In a simulation environment, a theoretical telecentric motionless point is assumed to be set, displacement and posture information transmitted by a main manipulator is simulated, the control method of the robot provided by the embodiment is simulated, and it can be seen from fig. 7 that a track of the tail end of an execution instrument and a track of the tail end (equivalent to the tail end of a mechanical arm) of a UR robot TCP intersect at the theoretically designed telecentric motionless point RCM, which accords with an expected effect.
It should be noted that the steps illustrated in the above-described flow diagrams or in the flow diagrams of the figures may be performed in a computer system, such as a set of computer-executable instructions, and that, although a logical order is illustrated in the flow diagrams, in some cases, the steps illustrated or described may be performed in an order different than here.
In addition, in combination with the control method of the robot provided in the above embodiment, a storage medium may also be provided to implement in the present embodiment. The storage medium having stored thereon a computer program; the computer program, when executed by a processor, implements the control method of any of the robots in the above embodiments.
It should be understood that the specific embodiments described herein are merely illustrative of this application and are not intended to be limiting. All other embodiments, which can be derived by a person skilled in the art from the examples provided herein without any inventive step, shall fall within the scope of protection of the present application.
It is obvious that the drawings are only examples or embodiments of the present application, and it is obvious to those skilled in the art that the present application can be applied to other similar cases according to the drawings without creative efforts. Moreover, it should be appreciated that in the development of any such actual implementation, as in any engineering or design project, numerous implementation-specific decisions must be made to achieve the developers' specific goals, such as compliance with system-related and business-related constraints, which may vary from one implementation to another.
The term "embodiment" is used herein to mean that a particular feature, structure, or characteristic described in connection with the embodiment can be included in at least one embodiment of the present application. The appearances of such phrases in various places in the specification are not necessarily all referring to the same embodiment, nor are separate or alternative embodiments mutually exclusive of other embodiments. It is to be expressly and implicitly understood by one of ordinary skill in the art that the embodiments described in this application may be combined with other embodiments without conflict.
The above-mentioned embodiments only express several embodiments of the present application, and the description thereof is more specific and detailed, but not construed as limiting the scope of the patent protection. It should be noted that, for a person skilled in the art, several variations and modifications can be made without departing from the concept of the present application, which falls within the scope of protection of the present application. Therefore, the protection scope of the present application should be subject to the appended claims.
Claims (10)
1. A control method of a robot, the robot comprises a driven mechanical arm, an execution connecting rod is fixedly connected to the tail end of the mechanical arm of the driven mechanical arm, and an execution instrument is arranged at the tail end of the execution connecting rod, and the control method of the robot is characterized by comprising the following steps:
step 1, determining a point on the execution connecting rod as a telecentric motionless point of the driven mechanical arm, and determining the position information of the telecentric motionless point;
step 2, determining the pose information of the tail end of the execution instrument at the current position, and determining the position information of the tail end of the execution instrument at the target position based on the displacement increment of the tail end of the execution instrument provided by a main manipulator and the position information of the pose information of the tail end of the execution instrument at the current position;
and 3, determining a pose conversion relation of the tail end of the mechanical arm moving from the current position to the target position by taking the telecentric fixed point holding fixed position as constraint on the basis of pose information of the tail end of the execution instrument at the current position and position information of the tail end of the execution instrument at the target position, resolving the pose conversion relation to obtain joint angles of all joints of the driven mechanical arm on the basis of inverse kinematics, and controlling the driven mechanical arm on the basis of the joint angles of all the joints of the driven mechanical arm.
2. The method of controlling a robot according to claim 1, wherein determining the position information of the tip of the execution tool at the target position based on the displacement increment provided by the master manipulator and the position information of the tip of the execution tool in the pose information of the current position comprises:
acquiring displacement increment provided by the main manipulator;
sequentially decomposing the displacement increment into a plurality of target displacement increments according to the motion sequence;
determining position information of a plurality of target positions which are sequentially arranged according to a motion sequence based on each target displacement increment and the position information of the tail end of the execution instrument at the current position;
wherein, for each target position, the step 3 is executed in turn according to the motion sequence.
3. The method of controlling a robot of claim 2, wherein sequentially decomposing the displacement increments into a plurality of target displacement increments in a motion order comprises:
decomposing the displacement increment into a plurality of target displacement increments according to a preset step length or a preset interpolation position number under the condition that the displacement increment is larger than a second preset threshold;
and taking the displacement increment as the target displacement increment when the displacement increment is smaller than the second preset threshold.
4. The method of controlling a robot according to claim 1, wherein after the step 3, the method further comprises:
determining target pose information of the executing instrument tail end at a target position based on the posture information provided by the main operator and the position information of the executing instrument tail end at the target position;
and calculating joint angles of all joints of the execution instrument according to the initial pose information and the target pose information of the tail end of the execution instrument on the basis of inverse kinematics, and controlling the execution instrument on the basis of the joint angles of all the joints of the execution instrument.
5. The method of controlling a robot according to any one of claims 1 to 4, wherein the position of the end of the implement instrument is located on an extension line of the implement link; determining a pose transformation relationship of the mechanical arm end moving from the current position to the target position based on the pose information of the executing instrument end at the current position and the position information of the executing instrument end at the target position and taking the telecentric fixed point as a constraint comprises the following steps:
determining posture information of the tail end of the mechanical arm at the target position based on the target position of the tail end of the execution instrument and the posture of the straight line passing through the telecentric fixed point;
determining positional information of the robotic arm tip at a target location on the straight line based on a length of the performing instrument tip to the robotic arm tip;
and determining the posture conversion relation of the tail end of the mechanical arm moving from the current position to the target position according to the posture information of the tail end of the mechanical arm at the current position, the posture information of the tail end of the mechanical arm at the target position and the position information of the tail end of the mechanical arm at the target position.
6. The method according to any one of claims 1 to 4, wherein a posture of the effector tip with respect to the robot arm tip is fixed while the slave robot arm is controlled based on a joint angle of each joint of the slave robot arm.
7. The control method of a robot according to any one of claims 1 to 4,
the telecentric motionless point is arranged at a preset position on the execution connecting rod, and the position information of the telecentric motionless point is determined based on the preset position and the pose information of the tail end of the mechanical arm at the current position; or
The position information of the telecentric motionless point is determined based on the measurement.
8. The method of controlling a robot according to claim 7, wherein in a case where the positional information of the telecentric motionless point is determined based on measurement, the method further comprises:
determining pose information of the tail end of the mechanical arm at the current position;
measuring the length from the tail end of the mechanical arm to a preset position on the execution connecting rod;
and determining the position information of the preset position based on the length and the pose information of the tail end of the mechanical arm at the current position, and taking the position information of the preset position as the position information of the telecentric motionless point.
9. A robot comprising a master manipulator, a slave manipulator and a control device, characterized in that an actuator link is fixedly connected to a manipulator end of the slave manipulator, said actuator link end being provided with an actuator means, the control device comprising a memory in which a computer program is stored and a processor arranged to run the computer program to perform the steps of the method of controlling a robot according to any of claims 1 to 8.
10. A computer-readable storage medium, on which a computer program is stored, which, when being executed by a processor, carries out the steps of the control method of a robot according to any one of claims 1 to 8.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110884890.7A CN115703227A (en) | 2021-08-03 | 2021-08-03 | Robot control method, robot, and computer-readable storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110884890.7A CN115703227A (en) | 2021-08-03 | 2021-08-03 | Robot control method, robot, and computer-readable storage medium |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115703227A true CN115703227A (en) | 2023-02-17 |
Family
ID=85178480
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110884890.7A Pending CN115703227A (en) | 2021-08-03 | 2021-08-03 | Robot control method, robot, and computer-readable storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115703227A (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117084798A (en) * | 2023-10-19 | 2023-11-21 | 苏州恒瑞宏远医疗科技有限公司 | Puncture control method, puncture control device, computer equipment and storage medium |
| CN117549328A (en) * | 2024-01-08 | 2024-02-13 | 武汉联影智融医疗科技有限公司 | Positioning system and method of surgical robot and surgical robot system |
-
2021
- 2021-08-03 CN CN202110884890.7A patent/CN115703227A/en active Pending
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117084798A (en) * | 2023-10-19 | 2023-11-21 | 苏州恒瑞宏远医疗科技有限公司 | Puncture control method, puncture control device, computer equipment and storage medium |
| CN117084798B (en) * | 2023-10-19 | 2023-12-29 | 苏州恒瑞宏远医疗科技有限公司 | Puncture control method, puncture control device, computer equipment and storage medium |
| CN117549328A (en) * | 2024-01-08 | 2024-02-13 | 武汉联影智融医疗科技有限公司 | Positioning system and method of surgical robot and surgical robot system |
| CN117549328B (en) * | 2024-01-08 | 2024-04-02 | 武汉联影智融医疗科技有限公司 | Positioning system and method of surgical robot and surgical robot system |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US20200338730A1 (en) | Trajectory planning device, trajectory planning method and program | |
| CN107414842B (en) | Control device, robot, and robot system | |
| US9827675B2 (en) | Collision avoidance method, control device, and program | |
| US9387589B2 (en) | Visual debugging of robotic tasks | |
| US20150273689A1 (en) | Robot control device, robot, robotic system, teaching method, and program | |
| US10166673B2 (en) | Portable apparatus for controlling robot and method thereof | |
| US20150151431A1 (en) | Robot simulator, robot teaching device, and robot teaching method | |
| US20150112482A1 (en) | Teaching system and teaching method | |
| US11059181B2 (en) | Control apparatus, robot, and robot system | |
| CN115703227A (en) | Robot control method, robot, and computer-readable storage medium | |
| Yepes et al. | Implementation of an Android based teleoperation application for controlling a KUKA-KR6 robot by using sensor fusion | |
| JP2007286976A (en) | Robot simulation apparatus | |
| KR101876845B1 (en) | Robot control apparatus | |
| JP2015054378A (en) | Information processing device, robot, scenario information creation method and program | |
| JP7259860B2 (en) | ROBOT ROUTE DETERMINATION DEVICE, ROBOT ROUTE DETERMINATION METHOD, AND PROGRAM | |
| CN111775145A (en) | Control system of series-parallel robot | |
| CN111093911A (en) | Robot system and method for operating the same | |
| Nandikolla et al. | Teleoperation robot control of a hybrid eeg-based bci arm manipulator using ros | |
| CN111496798B (en) | Robot conveyor belt tracking method, equipment and storage device | |
| WO2022127650A1 (en) | Surgical robot and control method and control apparatus thereof | |
| CN113733107B (en) | Robot drag teaching method, robot and computer storage medium | |
| JP7249221B2 (en) | SENSOR POSITION AND POSTURE CALIBRATION DEVICE AND SENSOR POSITION AND POSTURE CALIBRATION METHOD | |
| CN110253568B (en) | Robot control method, system, device and storage medium | |
| Sun et al. | Direct virtual-hand interface in robot assembly programming | |
| US20220226982A1 (en) | Method Of Creating Control Program For Robot, System Executing Processing Of Creating Control Program For Robot, And Non-Transitory Computer-Readable Storage Medium |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |