CN115890749A - Endoscope robot visual servo and optimization control method and system under RCM constraint and robot - Google Patents

Endoscope robot visual servo and optimization control method and system under RCM constraint and robot Download PDF

Info

Publication number
CN115890749A
CN115890749A CN202211410907.6A CN202211410907A CN115890749A CN 115890749 A CN115890749 A CN 115890749A CN 202211410907 A CN202211410907 A CN 202211410907A CN 115890749 A CN115890749 A CN 115890749A
Authority
CN
China
Prior art keywords
robot
constraint
rcm
angle
endoscope
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
Application number
CN202211410907.6A
Other languages
Chinese (zh)
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.)
Sun Yat Sen University
Original Assignee
Sun Yat Sen University
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 Sun Yat Sen University filed Critical Sun Yat Sen University
Priority to CN202211410907.6A priority Critical patent/CN115890749A/en
Publication of CN115890749A publication Critical patent/CN115890749A/en
Pending legal-status Critical Current

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

Landscapes

  • Endoscopes (AREA)

Abstract

The invention discloses a visual servo and optimal control method, a system and a robot of an endoscope robot under RCM constraint, comprising the following steps: step one, completing a target detection task in a surgical field and extracting a current state of a target; step two, establishing a constraint optimization control scheme according to a specific robot model; step three, equivalently converting the constraint optimization problem into a nonlinear equation set based on a projection operator; step four, an error function is built by utilizing the nonlinear equation set obtained in the step three, and a recurrent neural network RNN solver is designed based on the Dini derivative so as to obtain an optimal solution of the constraint optimization problem; and step five, sending the joint angle or the joint speed obtained in the step four to a lower computer, driving the robot to move, and automatically adjusting the surgical field of vision. Compared with other methods, the method has the advantages that iterative computation is not needed, training is not needed, and partial coefficients and elements are continuous but non-conductive.

Description

Endoscope robot visual servo and optimization control method and system under RCM constraint and robot
Technical Field
The invention relates to the technical field of robots, in particular to a method, a system and a robot for controlling visual servo and optimization of an endoscope robot under RCM constraint.
Background
In the traditional minimally invasive surgery, a plurality of unstable factors exist in manual operation. For example, prolonged operation of an endoscope can cause the physician to shake their hands, resulting in an unstable field of view of the endoscope; in addition, there is a disadvantage that the communication and operation between the doctor and the assistant are not coordinated. In recent years, with the development of robotics and the improvement of medical level, medical robots have been successfully applied to the field of minimally invasive surgery. The robot-assisted minimally invasive surgery (RAMIS) system can effectively remove the influence of hand shaking during the operation of a doctor and improve the stability of the operation; the configuration of the operating personnel can be reduced, the fatigue of the operator can be relieved, and the labor cost of the operation can be reduced; the master-slave separation type operation mode can realize remote operation, sharing of medical resources and the like. In the research on the minimally invasive surgical robot, how to move the surgical instrument integrated at the end of the robot reliably around the small body surface incision as a fulcrum without enlarging the incision wound has become one of the research hotspots of the surgical robot.
A mechanism is said to be "telecentric" (RCM) if a part or point of the mechanism passes through a fixed point away from the mechanism itself throughout its movement and the point has no physical constraints in practice. The characteristics of the telecentric mechanism are in accordance with the operation requirements of minimally invasive surgery, so that the telecentric mechanism is widely applied to the minimally invasive surgery robot, and the motion performance and stability of the telecentric mechanism directly influence the operation performance of the whole minimally invasive surgery robot. Mechanical telecentric mechanisms typically have only one fixed center of focus, often requiring setup for a particular surgical procedure, and lack flexibility. In view of this, a software-based telecentric motion generation algorithm is proposed in the existing literature, which can be adjusted at a software level as required, and has higher flexibility and wider application range. During the operation, it is important to provide stable and timely visual information for doctors. The robot vision servo control is a method for capturing images in real time through a vision sensor, extracting characteristic information through an image processing algorithm, and driving a robot to operate by taking the characteristic information as a feedback signal.
The vision servo and the optimization control of the robot can be abstractly represented in mathematics into an optimization problem containing various constraints, such as a Quadratic Programming (QP) problem. Due to the characteristics of parallel computing, adaptivity, hardware realizability and the like, the neural network has become one of effective methods for solving the optimization problem. The existing QP solving algorithm mainly comprises a gradient-based Recurrent Neural Network (RNN), a zero-ized neural network ZNN and the like, wherein the RNN is suitable for solving the problem of invariance, and has the problems of more iteration times, long solving time and the like. While ZNN has the advantage of not requiring iterative computations, it requires that the relevant elements and coefficients be everywhere derivable and that it cannot directly handle the double-ended constraint. Based on the above analysis, we propose a new QP solution algorithm Dini-RNN in which double-ended constraints can be handled directly without iterative computations and in which some coefficients and elements are allowed to be continuous but not conductive.
Disclosure of Invention
The invention mainly aims to overcome the defects and shortcomings of the prior art and provides a visual servo and optimization control method, a system and a robot of an endoscope robot under RCM constraint.
In order to achieve the purpose, the invention adopts the following technical scheme:
in a first aspect, the present invention provides a visual servo and optimization control method for an endoscope robot under RCM constraint, including the following steps:
step one, completing a target detection task in an operation visual field, and extracting a target current state r;
step two, establishing a corresponding constraint optimization control scheme according to the specific endoscope robot, and designing a performance index of
Figure BDA0003938497850000011
The equation is constrained to->
Figure BDA0003938497850000012
And &>
Figure BDA0003938497850000013
And double-ended constraint of q - ≤q≤q + And &>
Figure BDA0003938497850000014
Where W denotes a weighting matrix q and +>
Figure BDA0003938497850000015
Respectively representing the angle and the rate of change of the angle of the driving joint, q - And q is + Respectively represent the upper and lower limits of the angle of the driving joint>
Figure BDA0003938497850000021
And &>
Figure BDA0003938497850000022
Respectively representing the upper and lower limits of the rate of change of the angle of the driving joint, J rcm Denotes the RCM point p rcm To the Jacobian matrix, J system Is a Jacobian matrix of the endoscope robot>
Figure BDA0003938497850000023
And &>
Figure BDA0003938497850000024
Respectively representing the linear velocity of an RCM point and the characteristic point velocity under a camera plane; two double-end constraints on the angle of the driving joint and the angle change rate thereof are collated into a double-end constraint->
Figure BDA0003938497850000025
Wherein->
Figure BDA0003938497850000026
And beta > 0, and then by introducing a matrix->
Figure BDA0003938497850000027
n > 5 and->
Figure BDA0003938497850000028
Figure BDA0003938497850000029
Uniformly representing a constraint optimization control scheme into a quadratic programming problem with a general form, wherein the performance index is
Figure BDA00039384978500000210
The constraint condition is->
Figure BDA00039384978500000211
Step three, by introducing a projection operator
Figure BDA00039384978500000212
Wherein the concrete expression of the projection operator is defined as
Figure BDA00039384978500000213
Equivalently converting the constraint optimization problem into a nonlinear equation set g (t, x, mu) =0 based on the KKT condition;
step four, inspiring by the design idea of the zero neural network, introducing the Dini derivative to obtain a brand new RNN solver
Figure BDA00039384978500000214
Where gamma is a convergence parameter, phi (-) represents the activation function array,
Figure BDA00039384978500000215
Figure BDA00039384978500000216
Figure BDA00039384978500000217
in the form of a diagonal matrix,
z＝Cx+μ，
Figure BDA00039384978500000218
Figure BDA00039384978500000219
is relative to z i The Dini upper right derivative operator is solved through an RNN solver to obtain an optimal solution of the constraint optimization problem, and then the angle change rate (or greater than or equal to) of the driving joint of the endoscope robot is obtained>
Figure BDA00039384978500000220
Step five, comparing the result obtained in the step four
Figure BDA00039384978500000221
Or the joint angle q is driven to be sent to a lower computer, the robot is driven to move, and the operation visual field is automatically adjusted.
Preferably, the endoscope robot comprises a fixed platform, an end effector, an endoscope and n driving joints q 1 ～q n N is more than 5, and the kinematic equation of the mechanical arm speed layer of the endoscope robot is
Figure BDA00039384978500000222
Wherein
Figure BDA0003938497850000031
J denotes the Jacobian matrix->
Figure BDA0003938497850000032
Representing the camera plane feature point velocity and the linear velocity at the RCM point.
As a preferred technical solution, the jacobian matrix is specifically:
Figure BDA0003938497850000033
wherein, J system ＝J image J camera ，J image Is a jacobian matrix of the image,
Figure BDA0003938497850000034
J task is a jacobian matrix of the endoscope tip relative to a base coordinate system, 0 J n and 0 J n+1 are respectively the Jacobian matrix of the nth coordinate system and the (n + 1) th coordinate system to the base coordinate system, 0 R n+1 is the rotation matrix of the (n + 1) th coordinate system with respect to the base coordinate system, λ ∈ (0,1).
As a preferred technical scheme, in the kinematic equation of the robot velocity layer, an optimized performance index is designed
Figure BDA0003938497850000035
Considering the upper and lower limit constraints of the angle of the driving joint and the change rate thereof, and establishing a constraint optimization control scheme of the endoscope robot;
after equivalent arrangement, the method is represented mathematically as a quadratic programming problem with a general form, wherein the performance index is
Figure BDA0003938497850000036
The constraint condition is->
Figure BDA0003938497850000037
Based on the projection operator and the KKT condition, the quadratic programming problem is converted equivalently to a nonlinear equation system g (t, x, μ) =0.
As a preferred technical solution, the projection operator is continuous everywhere.
As an optimal technical scheme, a brand new RNN solver is obtained by being inspired by the design idea of a zero neural network
Figure BDA0003938497850000038
Among them are:
Figure BDA0003938497850000039
Figure BDA00039384978500000310
Figure BDA00039384978500000311
in the form of a diagonal matrix,
Figure BDA00039384978500000312
z＝Cx+μ，
Figure BDA0003938497850000041
as an optimal technical scheme, the RNN solver does not need training and iterative computation, can efficiently solve and obtain the optimal solution of a quadratic programming problem, and further obtains the angle change rate of the driving joint of the endoscope robot
Figure BDA0003938497850000042
By varying the rate of change of angle of the drive joint>
Figure BDA0003938497850000043
Or the driving joint angle q obtained by integration is sent to a lower computer to drive the robot to move for operation visionAnd (4) automatically adjusting the field.
In a second aspect, the invention also provides an endoscope robot visual servo and optimization control system under the RCM constraint, which is applied to the endoscope robot visual servo and optimization control method under the RCM constraint and comprises a target detection module, a kinematics control scheme construction module, an equivalence transformation module, an optimization problem solving module and a driving module;
the target detection module is used for restricting a motion track, namely the ideal speed of the characteristic point under the camera plane of the endoscope robot
Figure BDA0003938497850000044
Setting (2);
the kinematic control scheme construction module is used for establishing a corresponding constraint optimization control scheme according to a specific endoscope robot, and the design performance index is
Figure BDA0003938497850000045
The equation is constrained to->
Figure BDA0003938497850000046
And &>
Figure BDA0003938497850000047
And double-ended constraint of q - ≤q≤q + And &>
Figure BDA0003938497850000048
Wherein W denotes a weighting matrix q and->
Figure BDA0003938497850000049
Respectively representing the angle and the rate of change of the angle of the driving joint, q - And q is + Respectively represent the upper and lower limits of the angle of the driving joint>
Figure BDA00039384978500000410
And &>
Figure BDA00039384978500000411
Respectively representing the upper and lower limits of the rate of change of the angle of the driving joint, J rcm Denotes the RCM point p rcm To form a Jacobian matrix, J system For the endoscopic robot Jacobian matrix, in combination with a camera>
Figure BDA00039384978500000412
And &>
Figure BDA00039384978500000413
Respectively representing the linear velocity of an RCM point and the characteristic point velocity under a camera plane; two double-end constraints on the angle of the driving joint and the angle change rate thereof are collated into a double-end constraint->
Figure BDA00039384978500000414
Wherein
Figure BDA00039384978500000415
And beta > 0, and then by introducing a matrix
Figure BDA00039384978500000416
n > 5 and->
Figure BDA00039384978500000417
Uniformly characterizing a constrained optimal control scheme as a quadratic programming problem having a general form in which a performance criterion is ^ based>
Figure BDA00039384978500000418
With a constraint of >>
Figure BDA00039384978500000419
The equivalence conversion module is used for projecting operator-based
Figure BDA00039384978500000420
And KKT condition, equivalently converting the constraint optimization problem into a nonlinear equation set g (t, x, mu) =0;
the optimal solving module is used for defining an error monitoring function e (t) = g (t, x, mu), the error monitoring function is a nonlinear equation set obtained in the third step, is inspired by the design idea of a zero neural network, introduces Dini derivatives, and obtains a brand new RNN solver
Figure BDA00039384978500000421
Where γ is the convergence parameter, Φ (-) represents the activation function array,
Figure BDA0003938497850000051
/>
Figure BDA0003938497850000052
Figure BDA0003938497850000053
is a diagonal matrix of the two angles,
z＝Cx+μ，
Figure BDA0003938497850000054
Figure BDA0003938497850000055
is relative to z i The Dini upper right derivative operator is solved through an RNN solver to obtain an optimal solution of a constraint optimization problem, and then the angle change rate->
Figure BDA0003938497850000056
The drive module is used for transmitting the obtained result
Figure BDA0003938497850000057
Or the joint angle q is driven to be sent to a lower computer, the robot is driven to move, and the operation visual field is carried outAutomatic adjustment of (2).
In a third aspect, the present invention further provides a computer readable storage medium, in which a program is stored, and when the program is executed by a processor, the method for performing visual servoing and optimal control of an endoscope robot under RCM constraints is implemented.
In a fourth aspect, the present invention also provides an endoscope robot comprising:
at least one processor; and a memory communicatively coupled to the at least one processor; wherein the memory stores computer program instructions executable by the at least one processor to enable the at least one processor to perform the endoscopic robot vision servo and optimization control method under the RCM constraints.
Compared with the prior art, the invention has the following advantages and beneficial effects:
the present invention can effectively overcome the disadvantages of the general technology, and the ZNN solver proposed before this needs all derivative information of the relevant elements and coefficients, so the equality constraint and the inequality constraint need to have differentiable boundaries everywhere. In addition, when the ZNN solver considers double-ended constraints, the double-ended constraints need to be equivalently converted into inequality constraints for processing, and the number of neurons and the computational complexity of the neural network controller are increased in this way. The invention utilizes a projection operator to carry out inequality constraint processing and equivalent transformation of a quadratic programming problem, introduces Dini derivatives, combines an evolution rule and an activation function to obtain a brand new RNN solver, and the RNN solver can solve the optimal solution of the quadratic programming problem. After the optimal solution of the quadratic programming problem is obtained, the endoscope robot can be driven to move under the RCM constraint, and the automatic target tracking task is realized. The Dini-RNN solver algorithm of the present invention has the advantages of no need of iteration, direct processing of double-ended constraints, and allowing partial coefficients and elements to be continuous but non-conductive. The invention provides a control method for automatically tracking a target by using a visual servo of an endoscope robot under the constraint of RCM, which is convenient to operate, does not need training and iterative computation, and has standard operation.
Drawings
In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings needed to be used in the description of the embodiments are briefly introduced below, and it is obvious that the drawings in the following description are only some embodiments of the present application, and it is obvious for those skilled in the art to obtain other drawings based on these drawings without creative efforts.
FIG. 1 is a flow chart of visual servo and optimization control of an endoscopic robot under RCM constraints according to an embodiment of the present invention;
FIG. 2 is a model diagram of a simulated Franka Emika Panda mechanical arm according to an embodiment of the present invention.
FIG. 3 is a diagram of an RCM scheme according to an embodiment of the present invention.
Fig. 4 is a motion trajectory of a feature point on a camera plane at the end of the simulated Franka Emika Panda mechanical arm according to the embodiment of the present invention, starting from any 6 initial positions, and finally reaching the center of the camera plane.
FIG. 5 is an RCM error plot of a simulated Franka Emika Panda mechanical arm according to an embodiment of the present invention.
FIG. 6 is a graph showing the variation of joint angles of a Franka Emika Panda robot according to an embodiment of the present invention.
FIG. 7 is a graph of the rate of change of joint angle for a simulated Franka Emika Panda robotic arm in accordance with an embodiment of the present invention.
FIG. 8 is a schematic block diagram of a system for constrained motion planning and control of an endoscopic robot under the constraint of RCM in accordance with an embodiment of the present invention;
fig. 9 is a schematic structural diagram of a robot according to an embodiment of the present invention.
Detailed Description
In order to make the technical solution of the present invention better understood by those skilled in the art, the technical solution of the present invention will be clearly and completely described below with reference to the embodiments of the present invention and the accompanying drawings, it should be understood that the drawings are for illustrative purposes only and are not to be construed as limiting the patent. It is to be understood that the embodiments described are only a few embodiments of the present application and not all embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present application.
Reference in the specification to "an embodiment" means that a particular feature, structure, or characteristic described in connection with the embodiment can be included in at least one embodiment of the specification. The appearances of the phrase 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 explicitly and implicitly understood by one skilled in the art that the embodiments described herein can be combined with other embodiments.
Examples
As shown in fig. 1, the present embodiment is a method for controlling the visual servoing and optimization of an endoscope robot under the constraint of RCM, the method includes the following steps:
step one, completing a target detection task in an operation visual field, and extracting a target current state r;
step two, establishing a corresponding constraint optimization control scheme according to the specific endoscope robot, and finally obtaining a performance index in the constraint optimization control scheme as
Figure BDA0003938497850000061
The equation is constrained to->
Figure BDA0003938497850000062
Both ends are constrained to be->
Figure BDA0003938497850000063
In the second step, the constraint is carried out by the equation
Figure BDA0003938497850000064
Can make the characteristic point speed and RCM point speed in the camera plane
Figure BDA0003938497850000065
And its joint angular velocity>
Figure BDA0003938497850000066
In connection therewith, automatic tracking of target control under RCM constraints can be achieved. Is bound by an inequality->
Figure BDA0003938497850000067
Joint limit constraints on the joint angles of the endoscopic robot can be addressed.
Step three: projection-based operator
Figure BDA0003938497850000068
And equivalently converting the constraint optimization problem into a nonlinear equation system.
In the third step, corresponding Lagrangian functions are defined for the constraint optimization problem, on the basis, derivation is carried out to obtain the KKT condition, and the obtained KKT condition is used
Figure BDA0003938497850000069
And (5) processing, and finally equivalently converting the constraint optimization problem into a nonlinear equation system.
Step four: and (4) aiming at the nonlinear equation set in the step three, constructing an error function and designing an RNN solver to obtain the nonlinear equation set and an optimal solution of the constraint optimization problem, so as to obtain the speed of each joint of the endoscope robot.
Constructing an error monitoring function e (t) = g (t, x, mu) based on the nonlinear equation set in the step three, introducing Dini derivatives under the inspiration of the design idea of the zero-ized neural network, and obtaining a brand-new RNN solver
Figure BDA0003938497850000071
Where γ is the convergence parameter, Φ (-) represents the activation function array,
Figure BDA0003938497850000072
Figure BDA0003938497850000073
Figure BDA0003938497850000074
in the form of a diagonal matrix,
z＝Cx+μ，
Figure BDA0003938497850000075
Figure BDA0003938497850000076
is relative to z i The finally obtained RNN solver can simultaneously obtain the optimal solution of the solution and constraint optimization problem of the nonlinear equation set>
Figure BDA0003938497850000077
Can obtain the change rate of the driving angle of the endoscope robot>
Figure BDA0003938497850000078
Step five: combining the results of step four
Figure BDA0003938497850000079
Or the joint angle q is driven to be sent to a lower computer, the endoscope robot is driven to move, and the operation visual field is automatically adjusted.
As shown in FIG. 2, in the present embodiment, the endoscope robot includes a fixed platform, an end effector, an endoscope, and 7 rotary joints, O being the 7 rotary joints respectively 1 ,...,O 7 And endoscope O 8 The kinematic relation of the robot velocity layer under the RCM constraint is
Figure BDA00039384978500000710
Wherein->
Figure BDA00039384978500000711
Representing the camera plane feature point velocity and the RCM point velocity.
As shown in FIG. 3, the endoscope linkage is passed into the body through the insertion site and cannot be laterally displaced at the RCM site.
As shown in fig. 4, the detected target is initialized randomly to any position on the camera plane for 6 times, and finally the camera controls the mechanical arm to move through the visual servo, so that the detected target can be seen to move to the center of the camera plane, which shows that the method can realize automatic tracking of the target under the RCM constraint and has little deviation.
As shown in fig. 5, in which the solid line e x Shows the error of the RCM point of the simulated Panda mechanical arm in the X direction, and the dotted line e y Shows the error of the RCM point of the simulated Panda mechanical arm in the Y direction, and the dotted line e z The error in the Z-direction of the RCM point of the simulated Panda tandem robot arm is shown. Wherein during the execution of the task, the errors in three directions are all less than or equal to 1.5 multiplied by 10 -5 Meter, high accuracy Yu Yahao meter level accuracy.
As shown in FIG. 6, wherein q is 1 ,q 2 ,q 3 ,q 4 ,q 5 ,q 6 ,q 7 Respectively showing the first rotary joints O of the simulated Panda endoscope robot 1 And a second rotary joint O 2 And a third rotary joint O 3 And a fourth rotary joint O 4 Fifth rotary joint O 5 Sixth rotary joint O 6 And a seventh rotary joint O 7 The angle of (c). During task execution, the angles of the joints are changed continuously, and various motions occur corresponding to the mechanical arms.
As shown in fig. 7, in which,
Figure BDA0003938497850000081
respectively showing the first rotary joints O of the simulated Panda endoscope robot 1 And a second rotary joint O 2 And a third rotary joint O 3 And a fourth rotary joint O 4 Fifth rotary joint O 5 Sixth rotary joint O 6 And a seventh rotary joint O 7 Angle change rate of. As can be seen from fig. 7, in the process of task execution, the angle change rate of each joint angle can be guaranteed to change within a certain range, so that the effectiveness of the invention in double-end constraint processing and joint limit avoidance in the quadratic programming problem can be demonstrated.
As shown in fig. 8, in another embodiment of the present application, there is provided an endoscope robot vision servo and optimization control system 100 under RCM constraint, comprising an object detection module 101, a kinematic control scheme construction module 102, an equivalence transformation module 103, an optimal solution module 104, and a driving module 105;
the target detection module 101 is used for restricting a motion track, namely an ideal speed of a characteristic point under a camera plane of the endoscope robot
Figure BDA0003938497850000082
Setting (2);
the kinematic control scheme building module 102 is configured to build a corresponding constraint optimization control scheme according to a specific endoscope robot, where a performance index is designed to be
Figure BDA0003938497850000083
The equation is constrained to >>
Figure BDA0003938497850000084
And &>
Figure BDA0003938497850000085
And double-ended constraint of q - ≤q≤q + And &>
Figure BDA0003938497850000086
Wherein W denotes a weighting matrix q and->
Figure BDA0003938497850000087
Respectively representing the angle and the rate of change of the angle of the driving joint, q - And q is + Respectively represent the upper and lower limits of the angle of the driving joint>
Figure BDA0003938497850000088
And &>
Figure BDA0003938497850000089
Respectively representing the upper and lower limits of the rate of change of the angle of the driving joint, J rcm Denotes the RCM point p rcm To form a Jacobian matrix, J system Is a Jacobian matrix of the endoscope robot>
Figure BDA00039384978500000810
And &>
Figure BDA00039384978500000811
Respectively representing the linear velocity of an RCM point and the characteristic point velocity under a camera plane; two double-end constraints on the angle of the driving joint and the angle change rate thereof can be collated into a double-end constraint->
Figure BDA00039384978500000812
Wherein
Figure BDA00039384978500000813
And beta > 0, and then by introducing a matrix
Figure BDA00039384978500000814
n > 5 and>
Figure BDA00039384978500000815
uniformly characterizing a constrained optimal control scheme as a quadratic programming problem having a general form in which a performance criterion is ^ based>
Figure BDA00039384978500000816
The constraint condition is->
Figure BDA00039384978500000817
The equivalence transformation module 103 is configured to transform the projection-based image into an equivalent image
Figure BDA00039384978500000818
And KKT condition, equivalently converting the constraint optimization problem into a nonlinear equation set g (t, x, mu) =0;
the optimal solution module 104 is configured to define an error monitoring function e (t) = g (t, x, μ), where the error monitoring function is derived by introducing a Dini derivative into an obtained nonlinear equation set, as inspired by a design idea of a zero-valued neural network, to obtain a brand-new RNN solver
Figure BDA00039384978500000819
Where gamma is a convergence parameter, phi (-) represents the activation function array,
Figure BDA0003938497850000091
Figure BDA0003938497850000092
Figure BDA0003938497850000093
in the form of a diagonal matrix,
z＝Cx+μ，
Figure BDA0003938497850000094
Figure BDA0003938497850000095
is relative to z i The Dini upper right derivative operator is solved through an RNN solver to obtain an optimal solution of the constraint optimization problem, and then the angle change rate (or greater than or equal to) of the driving joint of the endoscope robot is obtained>
Figure BDA0003938497850000096
The driving module 105 is used for obtaining the result
Figure BDA0003938497850000097
Or the integral q is sent to a lower computer to drive the endoscope robot to move.
In addition, in the implementation of the system for motion planning and control of an endoscopic robot under the RCM constraint of the above embodiment, the logical division of each program module is only an example, and in practical applications, the above function allocation may be performed by different program modules according to needs, for example, due to the configuration requirements of corresponding hardware or the convenience of implementation of software, that is, the visual servo of the endoscopic robot under the RCM constraint and the internal structure of the optimization control system are divided into different program modules to perform all or part of the above described functions.
As shown in fig. 9, in one embodiment, a robot 200 is provided, and the robot 200 may include a first processor 201, a first memory 202 and a bus, and may further include a computer program stored in the first memory 202 and executable on the first processor 201, such as a visual servo and optimization control program 203 of an endoscopic robot under the RCM constraint.
The first memory 202 includes at least one type of readable storage medium, which includes flash memory, removable hard disk, multimedia card, card-type memory (e.g., SD or DX memory, etc.), magnetic memory, magnetic disk, optical disk, etc. The first memory 202 may in some embodiments be an internal storage unit of the robot 200, e.g. a mobile hard disk of the robot 200. The first memory 202 may also be an external storage device of the robot 200 in other embodiments, such as a plug-in mobile hard disk, a Smart Media Card (SMC), a Secure Digital (SD) Card, a Flash memory Card (Flash Card), etc. provided on the robot 200. Further, the first memory 202 may also include both an internal storage unit of the robot 200 and an external storage device. The first memory 202 may be used not only to store application software installed in the robot 200 and various types of data, such as codes of a visual servo and optimization control program 203 of the endoscopic robot under the RCM constraint, but also to temporarily store data that has been output or will be output.
The first processor 201 may be composed of an integrated circuit in some embodiments, for example, a single packaged integrated circuit, or may be composed of a plurality of integrated circuits packaged with the same function or different functions, and includes one or more Central Processing Units (CPUs), microprocessors, digital Processing chips, graphics processors, and combinations of various control chips. The first processor 201 is a Control Unit (Control Unit) of the electronic device, connects various components of the whole electronic device by using various interfaces and lines, and executes various functions and processes data of the robot 200 by running or executing programs or modules (e.g., federal learning defense programs and the like) stored in the first memory 202 and calling data stored in the first memory 202.
Fig. 9 shows only a robot with components, and those skilled in the art will appreciate that the structure shown in fig. 8 does not constitute a limitation of the robot 200, and may include fewer or more components than those shown, or some components may be combined, or a different arrangement of components.
The endoscopic robot visual servoing and optimization control program 203 under RCM constraints stored in the first memory 202 of the robot 200 is a combination of instructions that, when executed in the first processor 201, can implement:
step one, completing a target detection task in an operation visual field, and extracting a target current state r;
step two, establishing a corresponding constraint optimization control scheme according to the specific endoscope robot, and designing a performance index of
Figure BDA0003938497850000101
The equation is constrained to->
Figure BDA0003938497850000102
And &>
Figure BDA0003938497850000103
And double-ended constraint of q - ≤q≤q + And
Figure BDA0003938497850000104
where W denotes a weighting matrix q and +>
Figure BDA0003938497850000105
Respectively representing the angle and the rate of change of the angle of the driving joint, q - And q is + Respectively represent the upper and lower limits of the angle of the driving joint>
Figure BDA0003938497850000106
And &>
Figure BDA0003938497850000107
Respectively representing the upper and lower limits of the rate of change of the angle of the driving joint, J rcm Denotes the RCM point p rcm To the Jacobian matrix, J system Is a Jacobian matrix of the endoscope robot>
Figure BDA0003938497850000108
And &>
Figure BDA0003938497850000109
Respectively representing the linear velocity of an RCM point and the characteristic point velocity under a camera plane; two double-end constraints on the angle of the driving joint and the angle change rate thereof are arranged into a double-end constraint->
Figure BDA00039384978500001010
Wherein
Figure BDA00039384978500001011
And beta > 0, and then by introducing a matrix
Figure BDA00039384978500001012
n > 5 and->
Figure BDA00039384978500001013
Figure BDA00039384978500001014
Uniformly characterizing a constrained optimal control scheme as a quadratic programming problem having a general form in which a performance criterion is ^ based>
Figure BDA00039384978500001015
The constraint condition is->
Figure BDA00039384978500001016
Step three, by introducing a projection operator
Figure BDA00039384978500001017
Wherein the concrete expression of the projection operator is defined as
Figure BDA00039384978500001018
Equivalently converting the constraint optimization problem into a nonlinear equation set g (t, x, mu) =0 based on the KKT condition;
step four, inspiring by the design idea of the zero neural network, introducing the Dini derivative to obtain a brand new RNN solver
Figure BDA00039384978500001019
Where γ is the convergence parameter, Φ (-) represents the activation function array,
Figure BDA00039384978500001020
Figure BDA00039384978500001021
Figure BDA00039384978500001022
in the form of a diagonal matrix,
z＝Cx+μ，
Figure BDA0003938497850000111
Figure BDA0003938497850000112
is relative to z i The Dini upper right derivative operator is solved through an RNN solver to obtain an optimal solution of the constraint optimization problem, and then the angle change rate (or greater than or equal to) of the driving joint of the endoscope robot is obtained>
Figure BDA0003938497850000113
Step five, comparing the result obtained in the step four
Figure BDA0003938497850000114
Or the joint angle q is driven to be sent to a lower computer, the robot is driven to move, and the operation visual field is automatically adjusted.
Further, the modules/units integrated by the robot 200, if implemented in the form of software functional units and sold or used as independent products, may be stored in a non-volatile computer-readable storage medium. The computer-readable medium may include: any entity or device capable of carrying said computer program code, recording medium, U-disk, removable hard disk, magnetic disk, optical disk, computer Memory, read-Only Memory (ROM).
It will be understood by those skilled in the art that all or part of the processes of the methods of the embodiments described above may be implemented by a computer program, which may be stored in a non-volatile computer readable storage medium, and when executed, may include the processes of the embodiments of the methods described above. Any reference to memory, storage, database, or other medium used in the embodiments provided herein may include non-volatile and/or volatile memory, among others. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically Programmable ROM (EPROM), electrically Erasable Programmable ROM (EEPROM), or flash memory. Volatile memory can include Random Access Memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in a variety of forms such as Static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double Data Rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous Link DRAM (SLDRAM), rambus (Rambus) direct RAM (RDRAM), direct Rambus Dynamic RAM (DRDRAM), and Rambus Dynamic RAM (RDRAM), among others.
The technical features of the above embodiments can be arbitrarily combined, and for the sake of brevity, all possible combinations of the technical features in the above embodiments are not described, but should be considered as the scope of the present specification as long as there is no contradiction between the combinations of the technical features.
The above embodiments are preferred embodiments of the present invention, but the present invention is not limited to the above embodiments, and any other changes, modifications, substitutions, combinations, and simplifications which do not depart from the spirit and principle of the present invention should be construed as equivalents thereof, and all such changes, modifications, substitutions, combinations, and simplifications are intended to be included in the scope of the present invention.

Claims (10)

1. A visual servo and optimization control method of an endoscope robot under RCM constraint is characterized by comprising the following steps:
step one, completing a target detection task in an operation visual field, and extracting a target current state r;
step two, establishing a corresponding constraint optimization control scheme according to the specific endoscope robot, and designing the performance index as
Figure FDA0003938497840000011
The equation is constrained to->
Figure FDA0003938497840000012
And &>
Figure FDA0003938497840000013
And double-ended constraint of q - ≤q≤q + And &>
Figure FDA0003938497840000014
Wherein W denotes a weighting matrix q and->
Figure FDA0003938497840000015
Respectively representing the angle and the rate of change of the angle of the driving joint, q - And q is + Respectively represent the upper and lower limits of the angle of the driving joint>
Figure FDA0003938497840000016
And &>
Figure FDA0003938497840000017
Respectively representing the upper and lower limits of the rate of change of the angle of the driving joint, J rcm Represents the RCM point p rcm To form a Jacobian matrix, J system Is a Jacobian matrix of the endoscope robot>
Figure FDA0003938497840000018
And &>
Figure FDA0003938497840000019
Respectively representing the linear velocity of an RCM point and the characteristic point velocity under a camera plane; two double-end constraints on the angle of the driving joint and the angle change rate thereof are arranged into a double-end constraint->
Figure FDA00039384978400000110
Wherein->
Figure FDA00039384978400000111
And beta > 0, and then by introducing a matrix->
Figure FDA00039384978400000112
n > 5 and->
Figure FDA00039384978400000113
Figure FDA00039384978400000114
Uniformly characterizing a constrained optimal control scheme as a quadratic programming problem having a general form in which a performance criterion is ^ based>
Figure FDA00039384978400000115
The constraint condition is->
Figure FDA00039384978400000116
Step three, by introducing a projection operator
Figure FDA00039384978400000117
Wherein the concrete expression of the projection operator is defined as
Figure FDA00039384978400000118
Based on KKT conditions, equivalently converting the constraint optimization problem into a nonlinear equation set g (t, x, mu) =0;
step four, inspiring by the design idea of the zero neural network, introducing the Dini derivative to obtain a brand new RNN solver
Figure FDA00039384978400000119
Where γ is the convergence parameter, Φ (-) represents the activation function array,
Figure FDA00039384978400000120
Figure FDA00039384978400000121
Figure FDA00039384978400000122
in the form of a diagonal matrix,
z＝Cx+μ，
Figure FDA00039384978400000123
Figure FDA00039384978400000124
is relative to z i The Dini upper right derivative operator is solved through an RNN solver to obtain an optimal solution of the constraint optimization problem, and then the angle change rate (or greater than or equal to) of the driving joint of the endoscope robot is obtained>
Figure FDA00039384978400000125
Step five, comparing the result obtained in the step four
Figure FDA00039384978400000126
Or the joint angle q is driven to be sent to a lower computer, the robot is driven to move, and the operation visual field is automatically adjusted.
2. The RCM-constrained visual servo and optimization control method of an endoscopic robot according to claim 1, wherein the endoscopic robot comprises a fixed platform, an end effector, an endoscope, and n driving joints q 1 ～q n N is more than 5, and the kinematic equation of the mechanical arm speed layer of the endoscope robot is
Figure FDA00039384978400000127
Wherein->
Figure FDA00039384978400000128
J denotes the Jacobian matrix->
Figure FDA00039384978400000129
Representing the camera plane feature point velocity and the linear velocity at the RCM point.
3. The visual servo and optimization control method of an endoscopic robot under RCM constraint according to claim 2, characterized in that the jacobian matrix is specifically:
Figure FDA0003938497840000021
wherein, J system ＝J image J camera ，J image Is a jacobian matrix of the image,
Figure FDA0003938497840000022
J task is a jacobian matrix of the endoscope tip relative to a base coordinate system, 0 J n and 0 J n+1 are respectively the Jacobian matrix of the nth coordinate system and the (n + 1) th coordinate system to the base coordinate system, 0 R n+1 is the rotation matrix of the (n + 1) th coordinate system with respect to the base coordinate system, λ ∈ (0,1).
4. The RCM-constrained visual servo and optimization control method for endoscope robots according to claim 2, wherein optimization performance indexes are designed in the kinematic equation of the robot velocity layer
Figure FDA0003938497840000023
Considering the constraint of the driving joint angle and the upper and lower limits of the change rate of the driving joint angle, and establishing a constraint optimization control scheme of the endoscope robot;
through equivalent arrangement, the method is represented as a quadratic programming problem with a general form in mathematics, wherein the performance index is
Figure FDA0003938497840000024
The constraint condition is->
Figure FDA0003938497840000025
Based on the projection operator and the KKT condition, the quadratic programming problem is equivalently converted into a nonlinear equation set g (t, x, μ) =0.
5. The RCM-constrained endoscopic robot vision servoing and optimization control method according to claim 4, wherein the projection operator is continuous everywhere.
6. The visual servo and optimal control method of an endoscopic robot under RCM constraints as claimed in claim 1, wherein a novel RNN solver is obtained based on the concept of zero-degree neural network design
Figure FDA0003938497840000026
Among them are:
Figure FDA0003938497840000027
Figure FDA0003938497840000028
Figure FDA0003938497840000029
in the form of a diagonal matrix,
z＝Cx+μ，
Figure FDA00039384978400000210
/>
Figure FDA00039384978400000211
7. the visual servo and optimization control method for the endoscope robot under the RCM constraint of claim 6, wherein the RNN solver can efficiently solve and obtain the optimal solution of the quadratic programming problem without training and iterative computation, so as to obtain the angle change rate of the driving joint of the endoscope robot
Figure FDA00039384978400000212
By varying the rate of change of angle of the driving joint->
Figure FDA00039384978400000213
Or the driving joint angle q obtained by integration is sent to a lower computer to drive the robot to move, so that the operation visual field is automatically adjusted.
An endoscope robot visual servo and optimization control system under RCM constraint, which is applied to the endoscope robot visual servo and optimization control method under RCM constraint of any one of claims 1-7, and is characterized by comprising a target detection module, a kinematics control scheme construction module, an equivalent transformation module, an optimization problem solving module and a driving module;
the target detection module is used for restricting a motion track, namely the ideal speed of the characteristic point under the camera plane of the endoscope robot
Figure FDA00039384978400000214
Determination of (1);
the kinematic control scheme construction module is used for establishing a corresponding constraint optimization control scheme according to a specific endoscope robot, and the design performance index is
Figure FDA0003938497840000031
The equation is constrained to >>
Figure FDA0003938497840000032
And &>
Figure FDA0003938497840000033
And a double end constraint of q - ≤q≤q + And &>
Figure FDA0003938497840000034
Wherein W denotes a weighting matrix q and->
Figure FDA0003938497840000035
Respectively representing the angle and the rate of change of the angle of the driving joint, q - And q is + Respectively represent the upper and lower limits of the angle of the driving joint>
Figure FDA0003938497840000036
And &>
Figure FDA0003938497840000037
Respectively representing the upper and lower limits of the rate of change of the angle of the driving joint, J rcm Represents the RCM point p rcm To form a Jacobian matrix, J system For the endoscopic robot Jacobian matrix, in combination with a camera>
Figure FDA0003938497840000038
And &>
Figure FDA0003938497840000039
Respectively representing the linear velocity of an RCM point and the characteristic point velocity under a camera plane; two double-end constraints on the angle of the driving joint and the angle change rate thereof are collated into a double-end constraint->
Figure FDA00039384978400000310
Wherein
Figure FDA00039384978400000311
And beta > 0, recanalizationOver-introduced matrix
Figure FDA00039384978400000312
n > 5 and->
Figure FDA00039384978400000313
Uniformly characterizing a constrained optimal control scheme as a quadratic programming problem having a general form in which a performance criterion is ^ based>
Figure FDA00039384978400000314
With a constraint of >>
Figure FDA00039384978400000315
The equivalence conversion module is used for projecting operator based
Figure FDA00039384978400000316
And KKT condition, equivalently converting the constraint optimization problem into a nonlinear equation set g (t, x, mu) =0;
the optimal solving module is inspired by the design idea of the zero neural network, introduces Dini derivatives and obtains a brand new RNN solver
Figure FDA00039384978400000317
Where γ is the convergence parameter, Φ (-) represents the activation function array,
Figure FDA00039384978400000318
/>
Figure FDA00039384978400000319
Figure FDA00039384978400000320
in the form of a diagonal matrix,
z＝Cx+μ，
Figure FDA00039384978400000321
Figure FDA00039384978400000322
is relative to z i The Dini upper right derivative operator is solved through an RNN solver to obtain an optimal solution of a constraint optimization problem, and then the angle change rate->
Figure FDA00039384978400000323
The drive module is used for transmitting the obtained result
Figure FDA00039384978400000324
Or the joint angle q is driven to be sent to a lower computer, the robot is driven to move, and the operation visual field is automatically adjusted.
9. A computer-readable storage medium storing a program which, when executed by a processor, implements the RCM-constrained visual servoing and optimization control method for an endoscopic robot according to any one of claims 1 to 7.
10. An endoscopic robot, comprising:
at least one processor; and a memory communicatively coupled to the at least one processor; wherein the memory stores computer program instructions executable by the at least one processor to cause the at least one processor to perform the method of visual servoing and optimization control of an endoscopic robot under RCM constraints as defined in any one of claims 1 to 7.
CN202211410907.6A 2022-11-11 2022-11-11 Endoscope robot visual servo and optimization control method and system under RCM constraint and robot Pending CN115890749A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211410907.6A CN115890749A (en) 2022-11-11 2022-11-11 Endoscope robot visual servo and optimization control method and system under RCM constraint and robot

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211410907.6A CN115890749A (en) 2022-11-11 2022-11-11 Endoscope robot visual servo and optimization control method and system under RCM constraint and robot

Publications (1)

Publication Number Publication Date
CN115890749A true CN115890749A (en) 2023-04-04

Family

ID=86470502

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211410907.6A Pending CN115890749A (en) 2022-11-11 2022-11-11 Endoscope robot visual servo and optimization control method and system under RCM constraint and robot

Country Status (1)

Country Link
CN (1) CN115890749A (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116687452A (en) * 2023-07-28 2023-09-05 首都医科大学附属北京妇产医院 Early pregnancy fetus ultrasonic autonomous scanning method, system and equipment

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116687452A (en) * 2023-07-28 2023-09-05 首都医科大学附属北京妇产医院 Early pregnancy fetus ultrasonic autonomous scanning method, system and equipment
CN116687452B (en) * 2023-07-28 2023-11-03 首都医科大学附属北京妇产医院 Early pregnancy fetus ultrasonic autonomous scanning method, system and equipment

Similar Documents

Publication Publication Date Title
US20220009096A1 (en) Inverse kinematics solving method for redundant robot and redundant robot and computer readable storage medium using the same
CN111438688B (en) Robot correction method, robot correction device, computer equipment and storage medium
CN109048897B (en) Method for teleoperation of master-slave robot
CN114952868B (en) 7-degree-of-freedom SRS (sounding reference Signal) type mechanical arm control method and device and piano playing robot
US10899005B2 (en) Link-sequence mapping device, link-sequence mapping method, and program
CN115890749A (en) Endoscope robot visual servo and optimization control method and system under RCM constraint and robot
WO2023168849A1 (en) Mechanical arm motion capture method, medium, electronic device, and system
CN115213905B (en) Method and system for controlling position and pose of redundant mechanical arm and robot
CN115157262B (en) Obstacle avoidance and optimal control method and system for joint-limited redundant mechanical arm and robot
CN110682293A (en) Robot arm correction method, robot arm correction device, robot arm controller and storage medium
CN115179297A (en) Method and system for controlling joint limit of joint in combined obstacle avoidance mode through position and posture of surgical robot
CN113524201B (en) Active adjusting method and device for pose of mechanical arm, mechanical arm and readable storage medium
CN113084791B (en) Mechanical arm control method, mechanical arm control device and terminal equipment
CN116141330A (en) Robot motion control method and device, robot equipment and storage medium
CN115424701B (en) Bone surface follow-up technology for optimal path planning
CN116175569A (en) Method for determining relation model of hand-eye matrix, hand-eye calibration method and equipment
CN114851168B (en) Motion planning and control method and system for joint-limited redundant parallel mechanical arm and robot
CN114643582B (en) Model-free joint fault-tolerant control method and device for redundant mechanical arm
CN115741666A (en) Robot hand-eye calibration method, robot and robot operation method
CN114407011A (en) Special-shaped workpiece grabbing planning method, planning device and special-shaped workpiece grabbing method
CN117428791B (en) Inverse kinematics solving method and system for shoulder four-axis rehabilitation robot
CN116141341B (en) Method for realizing pointing action of five-degree-of-freedom mechanical arm meeting Cartesian space constraint
CN116787417A (en) Robot control method, apparatus, device and storage medium
CN114274141A (en) Robot, method and device for controlling robot, and computer-readable storage medium
CN116869654A (en) Surgical robot telecentric motion and man-machine interaction admittance control method and system based on quadratic programming and robot

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