CN115781699B - Telecentric constrained robot motion planning method, system, equipment and medium - Google Patents

Telecentric constrained robot motion planning method, system, equipment and medium Download PDF

Info

Publication number
CN115781699B
CN115781699B CN202310070152.8A CN202310070152A CN115781699B CN 115781699 B CN115781699 B CN 115781699B CN 202310070152 A CN202310070152 A CN 202310070152A CN 115781699 B CN115781699 B CN 115781699B
Authority
CN
China
Prior art keywords
robot
telecentric
constraint
constructing
constrained
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202310070152.8A
Other languages
Chinese (zh)
Other versions
CN115781699A (en
Inventor
周雪峰
徐智浩
唐观荣
欧阳旻
吴鸿敏
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Institute of Intelligent Manufacturing of Guangdong Academy of Sciences
Original Assignee
Institute of Intelligent Manufacturing of Guangdong Academy of Sciences
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 Institute of Intelligent Manufacturing of Guangdong Academy of Sciences filed Critical Institute of Intelligent Manufacturing of Guangdong Academy of Sciences
Priority to CN202310070152.8A priority Critical patent/CN115781699B/en
Publication of CN115781699A publication Critical patent/CN115781699A/en
Application granted granted Critical
Publication of CN115781699B publication Critical patent/CN115781699B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Abstract

The invention discloses a telecentric constrained robot motion planning method, a system, equipment and a medium, which relate to the technical field of medical appliances, and the method comprises the following steps: constructing a kinematic model of the robot passing through the telecentric end constraint point according to the established telecentric end constraint point; constructing a problem model of low-speed optimization of the robot by constructing control conditions of an end effector task of the robot and constraint conditions of a telecentric end constraint point of the robot, and considering actual physical constraint of a system; constructing an iterative solving algorithm to solve the angular acceleration command in real time, and updating the angular speed according to the angular acceleration command; the angular velocity is output to control the robot. The invention can avoid the inverse of the Jacobian matrix of the system with excessively complex calculation and avoid the practical physical constraint overrun of the system.

Description

Telecentric constrained robot motion planning method, system, equipment and medium
Technical Field
The invention relates to the technical field of medical instruments, in particular to a telecentric constrained robot motion planning method, a telecentric constrained robot motion planning system, telecentric constrained robot motion planning equipment and a telecentric constrained robot motion planning medium.
Background
The need for minimally invasive surgery and high precision work presents a significant challenge to the motion planning control of such robots: on the one hand, the robot end needs to realize high-precision positioning and motion tracking so as to realize high-quality operation, and on the other hand, the robot cannot generate displacement on a wound (namely a distal end), so that patients are prevented from being unnecessarily wounded. These all present significant challenges to the motion and control of such robots.
Document CN113334357a proposes a hybrid robot system and a virtual RCM motion control method, but the method does not consider the actual constraint problem of the system; document CN113180828A proposes a method of constrained motion control of a surgical robot based on spin theory, which only considers the motion planning of the end effector, and ignores the model that considers the robot.
Disclosure of Invention
Aiming at the problem of real-time control of a telecentric constrained robot in the prior art, the invention provides a telecentric constrained robot motion planning method, a telecentric constrained robot motion planning system, telecentric constrained robot motion planning equipment and a telecentric constrained robot motion planning medium.
In order to achieve the above purpose, the present invention may be performed by the following technical scheme:
in a first aspect, the present invention provides a telecentric constrained robot motion planning method, comprising the steps of:
constructing a kinematic model of the robot passing through the telecentric end constraint point according to the established telecentric end constraint point;
constructing control conditions of an end effector task of the robot and constraint conditions of a distal constraint point of the robot;
constructing a problem model of low-speed optimization of the robot, and taking actual physical constraint of the system into consideration;
constructing an iterative solving algorithm to solve the angular acceleration command in real time, and updating the angular speed according to the angular acceleration command;
the angular velocity is output to control the robot.
The method for planning the motion of the robot constrained by the telecentric end, as described above, further constructs a kinematic model of the robot passing through the constraint point of the telecentric end, and specifically comprises the following steps: construction
Figure SMS_1
Position constraint and establish->
Figure SMS_2
Is not modified by the condition of (2)
Figure SMS_3
wherein
Figure SMS_4
The two sides are derived, and a kinematic model of the robot is introduced to obtain
Figure SMS_5
wherein ,
Figure SMS_7
for the position of the telecentric constraint point, +.>
Figure SMS_12
For the position and speed of the robot end effector, < >>
Figure SMS_15
Is the position at the start of the extreme connecting rod, +.>
Figure SMS_8
To describe parameters of the telecentric constraint point, +.>
Figure SMS_11
Is robot->
Figure SMS_14
Jacobian matrix with point correspondence, +.>
Figure SMS_17
Is robot->
Figure SMS_6
Jacobian matrix with point correspondence, +.>
Figure SMS_10
and />
Figure SMS_13
Respectively the robot joint angle, the angular velocity and the angular acceleration, due to + ->
Figure SMS_16
Is a fixed point, so that its time derivative is zero, and formula (2) is written as matrix form to obtain +.>
Figure SMS_9
Wherein the matrix
Figure SMS_18
。
The method for planning the motion of the robot constrained by the distal end, as described above, further comprises the following specific conditions for controlling the task of the end effector of the robot:
in order for the end of a robot to perform the intended operational task, the end effector needs to perform tracking of a given trajectory, i.e. the end
Figure SMS_19
Tracking the desired track +.>
Figure SMS_20
For this purpose, a tracking error is defined>
Figure SMS_21
And introducing positive control parameters +.>
Figure SMS_22
Establishing control conditions meeting the end effector tasks:
Figure SMS_23
wherein ,
Figure SMS_24
is the desired speed of movement of the robot.
The method for planning the motion of the robot constrained by the telecentric end, as described above, further comprises the following specific constraint conditions of the constraint point of the telecentric end:
introducing parameters describing telecentric end constraint points
Figure SMS_25
And according to->
Figure SMS_26
Movement cannot be generated on the constraint surface, and constraint conditions of a telecentric end constraint point are constructed: />
Figure SMS_27
Further deriving:
Figure SMS_28
according to the control condition of the end effector task and the constraint condition of the telecentric end constraint point, constructing a normalization matrix:
Figure SMS_29
wherein ,
Figure SMS_30
is the location of the distal constraint point.
The method for planning the motion of the robot constrained by the remote end further constructs a problem model of low-speed optimization of the robot and considers the actual physical constraint of the system, and specifically comprises the following steps:
Figure SMS_31
the method for planning the robot motion constrained by the distal end, as described above, further constructs an iterative solution algorithm to solve the angular acceleration instruction in real time, and specifically includes:
all physical constraints are built with a negative feedback control mechanism as differential inequality conditions:
Figure SMS_32
Figure SMS_33
is a control parameter of a negative feedback mechanism;
the control quantity is obtained by adopting the following iterative solving algorithm:
Figure SMS_34
wherein
Figure SMS_35
For controlling parameters +.>
Figure SMS_36
Is an auxiliary variable of the end effector, +.>
Figure SMS_37
Is a telecentric constraint point->
Figure SMS_38
Jacobian matrix of (1), wherein->
Figure SMS_39
Is a clipping function with a lower limit of +.>
Figure SMS_40
The upper limit is
Figure SMS_41
。
The method for planning the robot motion constrained by the distal end further updates the angular velocity according to the angular acceleration instruction, and specifically comprises the following steps:
the control amount of the robot according to equation (10) is updated in each control cycle:
Figure SMS_42
wherein
Figure SMS_43
For the control period.
In a second aspect, the present invention provides a telecentrically constrained robotic motion planning system comprising:
the processor is used for constructing a kinematic model of the robot penetrating through the telecentric end constraint point according to the established telecentric end constraint point, constructing a control condition of an end effector task of the robot and a constraint condition of the telecentric end constraint point of the robot, constructing a problem model of low-speed optimization of the robot, constructing an iterative solution algorithm to solve an angular acceleration instruction in real time by considering actual physical constraint of the system, and updating the angular speed according to the angular acceleration instruction;
and an actuator for outputting an angular velocity according to the processor to control the robot.
In a third aspect, the present invention provides an electronic device, including a processor and a memory, where the memory stores at least one instruction, at least one program, a code set, or an instruction set, where the at least one instruction, the at least one program, the code set, or the instruction set is loaded and executed by the processor to implement a telecentric constrained robot motion planning method as described above.
In a fourth aspect, the present invention provides a computer readable storage medium having stored therein at least one instruction, at least one program, a set of codes, or a set of instructions, the at least one instruction, the at least one program, the set of codes, or the set of instructions being loaded and executed by a processor to implement a telecentric constrained robot motion planning method as described above.
Compared with the prior art, the invention has the beneficial effects that:
1. the telecentric end constraint robot motion planning method can avoid the inverse of the Jacobian matrix of a system with excessively complex calculation, and simultaneously avoid the safety problems caused by the fact that the actual physical constraint of the system is overrun, such as joint amplitude limiting, joint angular velocity amplitude limiting, telecentric end constraint points must be on the last rod piece and the like.
2. The telecentric end constraint robot motion planning method provided by the embodiment of the invention has the advantage of enabling the system speed to be the most stable, and can avoid personnel accidental injury caused by how fast the robot is.
3. The telecentric constrained robot motion planning method of the embodiment of the invention realizes the description of the telecentric constraint points by adopting only one parameter, and has the advantages of simple description and convenient control and implementation.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the following description will briefly explain the drawings needed in the embodiments, and it is obvious that the drawings in the following description are only some embodiments of the present application, and that other drawings can be obtained according to these drawings without inventive effort for a person skilled in the art.
FIG. 1 is a schematic diagram of a telecentric constrained robotic motion planning system according to an embodiment of the invention;
FIG. 2 is a flow chart of a telecentric constrained robot motion planning method according to an embodiment of the invention;
FIG. 3 is an algorithm flow chart of a telecentric end constrained robot motion planning method according to an embodiment of the invention;
FIG. 4 is another schematic diagram of a telecentrically constrained robotic motion planning system according to an embodiment of the invention;
fig. 5 is a schematic structural diagram of an electronic device according to an embodiment of the present invention.
Detailed Description
The following description of the embodiments of the present invention will be made clearly and completely with reference to the accompanying drawings, in which it is apparent that the embodiments described are only some embodiments of the present application, but not all embodiments. All other embodiments, which can be made by one of ordinary skill in the art without undue burden from the present disclosure, are within the scope of the present disclosure.
Examples:
it should be noted that the terms "first," "second," and the like in the description and the claims of the present invention and the above figures are used for distinguishing between similar objects and not necessarily for describing a particular sequential or chronological order. It is to be understood that the data so used may be interchanged where appropriate such that the embodiments of the invention described herein may be implemented in sequences other than those illustrated or otherwise described herein. Furthermore, the terms "comprises," "comprising," and "having," and any variations thereof, are intended to cover a non-exclusive inclusion, such that a process, method, system, article, or apparatus that comprises a list of steps or elements is not necessarily limited to those steps or elements expressly listed or inherent to such process, method, article, or apparatus.
In the description of the present invention, the meaning of "plurality" means at least two, for example, two, three, etc., unless specifically defined otherwise. Furthermore, unless explicitly specified and limited otherwise, the terms "mounted," "connected," and "connected" are to be construed broadly, and may be either fixedly connected, detachably connected, or integrally connected, for example; can be mechanically or electrically connected; can be directly connected or indirectly connected through an intermediate medium, and can be communication between two elements. The specific meaning of the above terms in the present invention will be understood in specific cases by those of ordinary skill in the art.
The word "exemplary" is used hereinafter to mean "serving as an example, embodiment, or illustration. Any embodiment described as "exemplary" is not necessarily to be construed as preferred or advantageous over other embodiments.
In order to better understand the technical scheme provided by the embodiment of the invention, the difficulty faced by the real-time control of the robot constrained by the telecentric end of the embodiment of the invention is briefly introduced, the robot used for minimally invasive surgery and high-precision operation must track the expected track by the end effector to finish the given task of the robot, but in the operation process, the robot cannot generate displacement at a narrow cavity, namely the life safety of the robot body and a patient is ensured, however, under the dynamic complex task, the robot cannot adopt the mode of offline planning in advance-later repeated execution, and the complex constraint such as joint angle limiting exists in the system.
In order to better understand the technical solution provided by the embodiments of the present application, a few simple descriptions are provided below for application scenarios applicable to the technical solution provided by the embodiments of the present application, and it should be noted that the application scenarios described below are only used to illustrate the embodiments of the present application and are not limiting. In specific implementation, the technical scheme provided by the embodiment of the application can be flexibly applied according to actual needs.
Referring to fig. 1, fig. 1 shows a schematic diagram of a task of a robot with telecentric end constraint according to an embodiment of the invention, without loss of generality, assuming that the robot has
Figure SMS_44
Individual joints, thus the end positions thereofThe device can be described as->
Figure SMS_45
The starting point position of the terminal connecting rod is +.>
Figure SMS_46
. In the global coordinate system, the position of the telecentric constraint point is +.>
Figure SMS_47
. The end link of the robot must pass this point, assuming that the point on the end of the robot is +.>
Figure SMS_48
Then guarantee +.>
Figure SMS_49
。
Based on this, referring to fig. 1 to 3, the present invention provides a telecentric constrained robot motion planning method, which may include the following steps:
step 101: and constructing a kinematic model of the robot passing through the telecentric constraint point according to the given telecentric constraint point. Specifically, the method for constructing the kinematic model of the robot passing through the telecentric constraint point specifically comprises the following steps:
construction
Figure SMS_50
Position constraint and establish->
Figure SMS_51
Is not modified by the condition of (2)
Figure SMS_52
wherein />
Figure SMS_53
The two sides are derived, and a kinematic model of the robot is introduced to obtain
Figure SMS_54
wherein ,
Figure SMS_57
for the position of the telecentric constraint point, +.>
Figure SMS_59
For the position and speed of the robot end effector, < >>
Figure SMS_62
Is the position at the start of the extreme connecting rod, +.>
Figure SMS_58
To describe parameters of the telecentric constraint point, +.>
Figure SMS_60
Is robot->
Figure SMS_63
Jacobian matrix with point correspondence, +.>
Figure SMS_65
Is robot->
Figure SMS_55
Jacobian matrix with point correspondence, +.>
Figure SMS_61
and />
Figure SMS_64
Respectively the robot joint angle, the angular velocity and the angular acceleration, due to + ->
Figure SMS_66
Is a fixed point, so that the time derivative thereof is zero, and the formula (2) is written as a matrix form
Figure SMS_56
Wherein the matrix
Figure SMS_67
。
Step 102: and constructing control conditions of the end effector task of the robot and constraint conditions of the remote constraint points of the robot.
Specifically, the control conditions of the end effector task of the robot specifically include:
in order for the end of a robot to perform the intended operational task, the end effector needs to perform tracking of a given trajectory, i.e. the end
Figure SMS_68
Tracking the desired track +.>
Figure SMS_69
For this purpose, a tracking error is defined>
Figure SMS_70
And introducing positive control parameters +.>
Figure SMS_71
Establishing control conditions meeting the end effector tasks: />
Figure SMS_72
wherein ,
Figure SMS_73
is the desired speed of movement of the robot.
Further, the control conditions of the end effector task of the robot specifically include:
in order for the end of a robot to perform the intended operational task, the end effector needs to perform tracking of a given trajectory, i.e. the end
Figure SMS_74
Tracking the desired track +.>
Figure SMS_75
For this purpose, a tracking error is defined>
Figure SMS_76
And introducing positive control parameters +.>
Figure SMS_77
Establishing control conditions meeting the end effector tasks:
Figure SMS_78
wherein ,
Figure SMS_79
is the desired speed of movement of the robot.
Step 103: and constructing a problem model of low-speed optimization of the robot, and taking actual physical constraints of the system into consideration.
Specifically, a problem model of low-speed optimization of the robot is constructed, and actual physical constraints of the system (such as joint limiting, joint angular velocity limiting, and conditions that a distal constraint point must be at a last rod) are considered, which specifically include:
Figure SMS_80
。
step 104: and constructing an iterative solving algorithm to solve the angular acceleration command in real time, and updating the angular speed according to the angular acceleration command.
Specifically, a problem model of low-speed optimization of the robot is constructed, and actual physical constraints of the system are considered, specifically including:
Figure SMS_81
。
step 105: the angular velocity is output to control the robot.
Specifically, a problem model of low-speed optimization of the robot is constructed, and actual physical constraints of the system are considered, specifically including:
Figure SMS_82
。
the following table is the meaning of the characters appearing in the above formula
Figure SMS_83
。
Referring to fig. 4, based on the same inventive concept, an embodiment of the present invention further provides a telecentric constrained robot motion planning system, including: the system comprises a processor and an executor, wherein the processor is used for constructing a kinematic model of the robot penetrating through a given telecentric end constraint point, constructing control conditions of an end executor task of the robot and constraint conditions of the telecentric end constraint point of the robot, constructing a problem model of low-speed optimization of the robot, constructing an iterative solution algorithm to solve an angular acceleration instruction in real time according to actual physical constraint of the system, and updating the angular speed according to the angular acceleration instruction; the actuator is used for outputting the angular velocity according to the processor to control the robot.
Because the system is a system corresponding to the telecentric constrained robot motion planning method in the embodiment of the invention, and the principle of solving the problem of the system is similar to that of the method, the implementation of the system can refer to the implementation process of the embodiment of the method, and the repetition is omitted.
Referring to fig. 5, based on the same inventive concept, an embodiment of the present invention further provides an electronic device, where the electronic device includes a processor and a memory, where at least one instruction, at least one program, a code set, or an instruction set is stored in the memory, where the at least one instruction, the at least one program, the code set, or the instruction set is loaded and executed by the processor, so as to implement the telecentric constrained robot motion planning method as described above.
It is understood that the Memory may include random access Memory (Random Access Memory, RAM) or Read-Only Memory (RAM). Optionally, the memory includes a non-transitory computer readable medium (non-transitory computer-readable storage medium). The memory may be used to store instructions, programs, code sets, or instruction sets. The memory may include a stored program area and a stored data area, wherein the stored program area may store instructions for implementing an operating system, instructions for at least one function, instructions for implementing the various method embodiments described above, and the like; the storage data area may store data created according to the use of the server, etc.
The processor may include one or more processing cores. The processor uses various interfaces and lines to connect various portions of the overall server, perform various functions of the server, and process data by executing or executing instructions, programs, code sets, or instruction sets stored in memory, and invoking data stored in memory. Alternatively, the processor may be implemented in hardware in at least one of digital signal processing (Digital Signal Processing, DSP), field programmable gate array (Field-Programmable Gate Array, FPGA), programmable logic array (Programmable Logic Array, PLA). The processor may integrate one or a combination of several of a central processing unit (Central Processing Unit, CPU) and a modem etc. Wherein, the CPU mainly processes an operating system, application programs and the like; the modem is used to handle wireless communications. It will be appreciated that the modem may not be integrated into the processor and may be implemented by a single chip.
Because the electronic device is the electronic device corresponding to the telecentric constrained robot motion planning method in the embodiment of the invention, and the principle of solving the problem of the electronic device is similar to that of the method, the implementation of the electronic device can refer to the implementation process of the embodiment of the method, and the repetition is omitted.
Based on the same inventive concept, the embodiments of the present invention also provide a computer-readable storage medium having stored therein at least one instruction, at least one program, a set of codes, or a set of instructions, which are loaded and executed by a processor to implement the telecentric constrained robot motion planning method as described above.
Those of ordinary skill in the art will appreciate that all or part of the steps of the various methods of the above embodiments may be implemented by a program that instructs associated hardware, the program may be stored in a computer readable storage medium including Read-Only Memory (ROM), random access Memory (Random Access Memory, RAM), programmable Read-Only Memory (Programmable Read-Only Memory, PROM), erasable programmable Read-Only Memory (Erasable Programmable Read Only Memory, EPROM), one-time programmable Read-Only Memory (OTPROM), electrically erasable programmable Read-Only Memory (EEPROM), compact disc Read-Only Memory (Compact Disc Read-Only Memory, CD-ROM) or other optical disk Memory, magnetic disk Memory, tape Memory, or any other medium that can be used for carrying or storing data that is readable by a computer.
Because the storage medium is a storage medium corresponding to the telecentric constrained robot motion planning method according to the embodiment of the invention, and the principle of solving the problem by the storage medium is similar to that of the method, the implementation of the storage medium can refer to the implementation process of the embodiment of the method, and the repetition is omitted.
In some possible implementations, aspects of the method of the embodiments of the present invention may also be implemented in the form of a program product comprising program code for causing a computer device to carry out the steps of the telecentricity constrained robot motion planning method according to the various exemplary embodiments of the present application described hereinabove, when the program product is run on a computer device. Wherein executable computer program code or "code" for performing the various embodiments may be written in a high-level programming language such as C, C ++, c#, smalltalk, java, javaScript, visual Basic, structured query language (e.g., act-SQL), perl, or in a variety of other programming languages.
In the description of the present specification, a description referring to terms "one embodiment," "some embodiments," "examples," "specific examples," or "some examples," etc., means that a particular feature, structure, material, or characteristic described in connection with the embodiment or example is included in at least one embodiment or example of the present invention. In this specification, schematic representations of the above terms are not necessarily directed to the same embodiment or example. Furthermore, the particular features, structures, materials, or characteristics described may be combined in any suitable manner in any one or more embodiments or examples. Furthermore, the different embodiments or examples described in this specification and the features of the different embodiments or examples may be combined and combined by those skilled in the art without contradiction.
The above embodiments are only for illustrating the technical concept and features of the present invention, and are intended to enable those skilled in the art to understand the content of the present invention and implement the same, and are not intended to limit the scope of the present invention. All equivalent changes or modifications made in accordance with the essence of the present invention are intended to be included within the scope of the present invention.

Claims (10)

1. A remote constrained robot motion planning method is characterized by comprising the following steps:
constructing a kinematic model of the robot passing through the telecentric end constraint point according to the established telecentric end constraint point;
constructing control conditions of an end effector task of the robot and constraint conditions of a distal constraint point of the robot;
constructing a problem model of low-speed optimization of the robot, and taking actual physical constraint of the system into consideration;
constructing an iterative solving algorithm to solve the angular acceleration command in real time, and updating the angular speed according to the angular acceleration command;
the angular velocity is output to control the robot.
2. The telecentric constrained robot motion planning method of claim 1, wherein constructing the kinematic model of the robot passing through the telecentric constraint point specifically comprises:
construction
Figure QLYQS_1
Position constraint and establish->
Figure QLYQS_2
Is not modified by the condition of (2)
Figure QLYQS_3
（1）
wherein
Figure QLYQS_4
Deriving both sides of the formula (1), and introducing a kinematic model of the robot to obtain
Figure QLYQS_5
（2）
wherein ,
Figure QLYQS_7
for the position of the telecentric constraint point, +.>
Figure QLYQS_13
and />
Figure QLYQS_16
Position and speed of the robot end effector, < >, respectively>
Figure QLYQS_9
Is the position at the start of the extreme connecting rod, +.>
Figure QLYQS_11
To describe parameters of the telecentric constraint point, +.>
Figure QLYQS_15
Is robot->
Figure QLYQS_18
Jacobian matrix with point correspondence, +.>
Figure QLYQS_6
Is robot->
Figure QLYQS_10
Jacobian matrix with point correspondence, +.>
Figure QLYQS_14
、/>
Figure QLYQS_17
and />
Figure QLYQS_8
Respectively the robot joint angle, the angular velocity and the angular acceleration, due to + ->
Figure QLYQS_12
Is a fixed point, so that the time derivative thereof is zero, and the formula (2) is written as a matrix form
Figure QLYQS_19
（3）
Wherein the matrix
Figure QLYQS_20
。
3. The telecentric constrained robot motion planning method of claim 2, wherein the control conditions of the end effector task of the robot specifically comprise:
in order for the end of a robot to perform the intended operational task, the end effector needs to perform tracking of a given trajectory, i.e. the end
Figure QLYQS_21
Tracking the desired track +.>
Figure QLYQS_22
For this purpose, a tracking error is defined>
Figure QLYQS_23
And introducing positive control parameters +.>
Figure QLYQS_24
Establishing control conditions meeting the end effector tasks:
Figure QLYQS_25
（4）
wherein ,
Figure QLYQS_26
is the desired speed of movement of the robot.
4. A method for telecentric constrained robotic motion planning according to claim 3, characterized in that the constraints of the telecentric constraint points comprise:
introducing parameters describing telecentric end constraint points
Figure QLYQS_27
And according to->
Figure QLYQS_28
Movement cannot be generated on the constraint surface, and constraint conditions of a telecentric end constraint point are constructed:
Figure QLYQS_29
（5）
further deriving:
Figure QLYQS_30
（6）/>
according to the control condition of the end effector task and the constraint condition of the telecentric end constraint point, constructing a normalization matrix:
Figure QLYQS_31
（7）
wherein ,
Figure QLYQS_32
is the location of the distal constraint point.
5. The telecentric constrained robot motion planning method of claim 4, wherein constructing a problem model for low-speed optimization of the robot, taking into account the actual physical constraints of the system, comprises:
Figure QLYQS_33
。 （8）
6. the telecentric constrained robot motion planning method of claim 5, wherein constructing an iterative solution algorithm solves the angular acceleration instructions in real time, specifically comprising:
all physical constraints are built with a negative feedback control mechanism as differential inequality conditions:
Figure QLYQS_34
（9）
Figure QLYQS_35
is a control parameter of a negative feedback mechanism;
the control quantity is obtained by adopting the following iterative solving algorithm:
Figure QLYQS_36
（10）
wherein
Figure QLYQS_37
For controlling parameters +.>
Figure QLYQS_38
Is an auxiliary variable of the end effector, +.>
Figure QLYQS_39
Is a telecentric constraint point->
Figure QLYQS_40
Jacobian matrix of->
Figure QLYQS_41
Is a clipping function with a lower limit of
Figure QLYQS_42
The upper limit is->
Figure QLYQS_43
。
7. The telecentric constrained robot motion planning method of claim 6, wherein updating the angular velocity according to the angular acceleration command comprises:
the control amount of the robot according to equation (10) is updated in each control cycle:
Figure QLYQS_44
wherein />
Figure QLYQS_45
For the control period.
8. A telecentric constrained robotic motion planning system, comprising:
the processor is used for constructing a kinematic model of the robot penetrating through the telecentric end constraint point according to the established telecentric end constraint point, constructing a control condition of an end effector task of the robot and a constraint condition of the telecentric end constraint point of the robot, constructing a problem model of low-speed optimization of the robot, constructing an iterative solution algorithm to solve an angular acceleration instruction in real time by considering actual physical constraint of the system, and updating the angular speed according to the angular acceleration instruction;
and an actuator for outputting an angular velocity according to the processor to control the robot.
9. An electronic device comprising a processor and a memory, wherein the memory stores at least one instruction, at least one program, a set of codes, or a set of instructions, the at least one instruction, the at least one program, the set of codes, or the set of instructions being loaded and executed by the processor to implement the telecentrically constrained robotic motion planning method of any of claims 1-7.
10. A computer readable storage medium having stored therein at least one instruction, at least one program, code set, or instruction set, the at least one instruction, the at least one program, the code set, or instruction set being loaded and executed by a processor to implement the telecentric constrained robot motion planning method of any of claims 1 to 7.
CN202310070152.8A 2023-02-07 2023-02-07 Telecentric constrained robot motion planning method, system, equipment and medium Active CN115781699B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310070152.8A CN115781699B (en) 2023-02-07 2023-02-07 Telecentric constrained robot motion planning method, system, equipment and medium

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310070152.8A CN115781699B (en) 2023-02-07 2023-02-07 Telecentric constrained robot motion planning method, system, equipment and medium

Publications (2)

Publication Number Publication Date
CN115781699A CN115781699A (en) 2023-03-14
CN115781699B true CN115781699B (en) 2023-05-02

Family

ID=85430155

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310070152.8A Active CN115781699B (en) 2023-02-07 2023-02-07 Telecentric constrained robot motion planning method, system, equipment and medium

Country Status (1)

Country Link
CN (1) CN115781699B (en)

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP3705074A1 (en) * 2019-03-08 2020-09-09 MAKO Surgical Corp. Systems and methods for controlling movement of a surgical tool along a predefined path
CN110303501B (en) * 2019-07-25 2022-05-03 华侨大学 Noise-tolerant redundant manipulator acceleration layer obstacle avoidance planning method
CN112417755A (en) * 2020-11-10 2021-02-26 西安工程大学 Master-slave mode surgical robot track prediction control method
CN113341731B (en) * 2021-06-29 2023-03-24 西北工业大学 Space robot trajectory planning method based on sequence convex optimization

Also Published As

Publication number Publication date
CN115781699A (en) 2023-03-14

Similar Documents

Publication Publication Date Title
Qureshi et al. A supervisory on-line tuned fuzzy logic based sliding mode control for robotics: An application to surgical robots
CN111890353B (en) Robot teaching track reproduction method and device and computer readable storage medium
WO2018019111A1 (en) Robot and joint motion control method and device therefor
US20240050178A1 (en) Surgical robot, control method thereof, and storage medium
JP4760732B2 (en) Route creation device
CN110464470A (en) The control method of operating robot and its arm body, control device
CN115781699B (en) Telecentric constrained robot motion planning method, system, equipment and medium
Huang et al. 4-dof visual servoing of a robotic flexible endoscope with a predefined-time convergent and noise-immune adaptive neural network
Jha et al. Robot programming by demonstration using teleoperation through imitation
US20110264265A1 (en) Robot, control system, and recording medium for control program
CN112597612B (en) Robot optimization method, device, terminal equipment and computer readable storage medium
CN110039546A (en) For controlling the method and robot of robot motion
CN113524201A (en) Active adjusting method and device for pose of mechanical arm, mechanical arm and readable storage medium
Sun et al. Visual‐based autonomous field of view control of laparoscope with safety‐RCM constraints for semi‐autonomous surgery
Sagitov et al. Design of simple one-arm surgical robot for minimally invasive surgery
Zhou et al. An autonomous task algorithm based on behavior trees for robot
CN116141330A (en) Robot motion control method and device, robot equipment and storage medium
Muñoz et al. Control movement scheme based on manipulability concept for a surgical robotic assistant
CN115554568A (en) Catheter bending steering control method, catheter system, and storage medium
WO2022141268A1 (en) Humanoid robot control method, computer device, and storage medium
Dahroug et al. Unilaterally constrained motion of a curved surgical tool
CN113359462A (en) Bionic eye image stabilization system and method based on disturbance decoupling and compensation
Azizkhani et al. Design, modeling, and redundancy resolution of soft robot for effective harvesting
CN113069208B (en) Operation navigation method, device, electronic equipment and storage medium
CN114367989B (en) Default mechanical arm impedance control method and device, mechanical arm and 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
GR01 Patent grant
GR01 Patent grant