CN112417755A - Master-slave mode surgical robot track prediction control method - Google Patents

Master-slave mode surgical robot track prediction control method Download PDF

Info

Publication number
CN112417755A
CN112417755A CN202011249222.9A CN202011249222A CN112417755A CN 112417755 A CN112417755 A CN 112417755A CN 202011249222 A CN202011249222 A CN 202011249222A CN 112417755 A CN112417755 A CN 112417755A
Authority
CN
China
Prior art keywords
master
slave
hand
track
time
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
CN202011249222.9A
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.)
Xian Polytechnic University
Original Assignee
Xian Polytechnic 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 Xian Polytechnic University filed Critical Xian Polytechnic University
Priority to CN202011249222.9A priority Critical patent/CN112417755A/en
Publication of CN112417755A publication Critical patent/CN112417755A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/20—Design optimisation, verification or simulation
    • G06F30/27—Design optimisation, verification or simulation using machine learning, e.g. artificial intelligence, neural networks, support vector machines [SVM] or training a model
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
    • A61B34/30—Surgical robots
    • A61B34/37—Master-slave robots
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
    • A61B34/70—Manipulators specially adapted for use in surgery
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F2111/00—Details relating to CAD techniques
    • G06F2111/04—Constraint-based CAD
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F2111/00—Details relating to CAD techniques
    • G06F2111/06—Multi-objective optimisation, e.g. Pareto optimisation using simulated annealing [SA], ant colony algorithms or genetic algorithms [GA]
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F2111/00—Details relating to CAD techniques
    • G06F2111/10—Numerical modelling

Landscapes

  • Engineering & Computer Science (AREA)
  • Health & Medical Sciences (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Surgery (AREA)
  • Medical Informatics (AREA)
  • Robotics (AREA)
  • Physics & Mathematics (AREA)
  • Molecular Biology (AREA)
  • Veterinary Medicine (AREA)
  • Public Health (AREA)
  • General Health & Medical Sciences (AREA)
  • Theoretical Computer Science (AREA)
  • Animal Behavior & Ethology (AREA)
  • Evolutionary Computation (AREA)
  • Nuclear Medicine, Radiotherapy & Molecular Imaging (AREA)
  • Heart & Thoracic Surgery (AREA)
  • Biomedical Technology (AREA)
  • Artificial Intelligence (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Computer Hardware Design (AREA)
  • Software Systems (AREA)
  • General Physics & Mathematics (AREA)
  • General Engineering & Computer Science (AREA)
  • Geometry (AREA)
  • Feedback Control In General (AREA)
  • Manipulator (AREA)

Abstract

The invention discloses a trajectory prediction control method of a master-slave surgical robot, which comprises the steps of firstly designing a least square support vector machine Kalman vibration filtering algorithm, and filtering tremor signals of hands to obtain target trajectory signals; respectively planning the target track signal by a linear track planning method and a Chebyshev pseudo-spectrum method to obtain a planned track, and optimizing the planned track by a quadratic planning method to obtain an actual track signal; and mapping the motion poses of the master hand and the slave hand by taking the actual track signals as input through a master-slave pose mapping function, thereby realizing the predictive control of the track of the master-slave surgical robot. The invention discloses a track prediction control method for a master-slave surgical robot, which solves the problems of delay and low precision of the tail end operation of the master-slave surgical robot.

Description

Master-slave mode surgical robot track prediction control method
Technical Field
The invention belongs to the technical field of surgical robot control, and particularly relates to a master-slave type surgical robot track prediction control method.
Background
At present, the surgical robot technology becomes a research hotspot in the field of current medical robots, and has wide application prospects in the field of clinical surgical operations. The control of the operation track of the mobile mechanical arm refers to the generation of a discrete pose sequence from a starting point to a target point of the mobile mechanical arm end effector under the constraints of the mobile mechanical arm kinematics (displacement and speed) and the constraints of the dynamics (acceleration and driving moment). Nowadays, the following two main types of motion trajectory control of a mobile manipulator are available: (1) however, the planning method specifies a discrete pose sequence required by the tail end execution mechanism to meet the requirement of a specific path in the task space, and a large amount of matrix operations are required to convert task space parameters into control variables of joint spaces, such as linear planning, circular arc planning, polynomial curve planning and the like, so that the slave mobile phone mechanical arm cannot be controlled in real time. (2) The joint space planning method is generally limited only by joint speed and acceleration, can effectively avoid the problems of singular points and redundancy of the robot, and generally comprises polynomial interpolation, trapezoidal speed interpolation, spline curve interpolation and the like, but the trajectory of the planning method is not smooth enough.
The vibration of the tail end of the slave hand is mainly caused by the physiological vibration of the operator at the master hand end, the precision can be improved by filtering vibration signals in the whole control system of the master-slave surgical robot, and methods of low-pass filtering, zero-phase filtering and signal compensation are mainly adopted. The low-pass filtering is the simplest and easiest method, can basically filter out vibration signals, but can cause signal delay and influence the real-time performance of the operation. The zero-phase filtering can improve the problem of phase lag of the former, and the introduction of the reverse filtering can realize the filtering without the phase lag, but the whole system can perform time sequence inversion and cannot be applied to a real-time operating system. The motion precision and the real-time performance of the surgical robot are strict, and the simple trajectory planning cannot meet the working requirements of the surgical robot.
Disclosure of Invention
The invention aims to provide a track prediction control method for a master-slave surgical robot, which solves the problems of delay and low precision of the tail end operation of the master-slave surgical robot.
The technical scheme adopted by the invention is a master-slave mode surgical robot track prediction control method, which is implemented according to the following steps:
step 1, designing a least square support vector machine Kalman vibration filtering algorithm, and filtering tremor signals of hands to obtain target track signals;
step 2, respectively carrying out trajectory planning on the target trajectory signal by a linear trajectory planning method and a Chebyshev pseudo-spectrum method to obtain a planned trajectory, and optimizing the planned trajectory by a quadratic planning method to obtain an actual trajectory signal;
and 3, mapping the motion poses of the master hand and the slave hand by taking the actual track signal as input through a master-slave pose mapping function, and realizing the prediction control of the track of the master-slave surgical robot.
The invention is also characterized in that:
the step 1 specifically comprises the following steps:
step 1.1, collecting the running track of a master hand, carrying out discretization calibration on the running track, and writing a Kalman filtering process into a state equation and a measurement process:
and (3) state process:
X(k)＝AX(k-1)+w(k) (1)，
the measurement process comprises the following steps:
Z(k)＝HX(k)+v(k) (2)，
in the formulas (1) and (2), x (k) is the state of the vibration signal in the main hand, the matrix a represents the state change, the vector z (k) is the signal measurement result, the matrix H represents the measurement change, the vector w (k) is the state noise, and the vector v (k) is the measurement noise, wherein w (k) and v (k) are gaussian white noises with the average value of 0, which are assumed to be independent of each other;
step 1.2, prediction
Predicting the value of the next time state according to the estimation value of the previous time state to become prior estimation, and predicting the error of the next time state to become prior error:
Figure BDA0002771048300000031
P(k|k-1)＝AP(k-1|k-1)AT+Q (4)，
in the formulas (3) and (4),
Figure BDA0002771048300000032
the state vector at time k is estimated from time k-1,
Figure BDA0002771048300000033
is the a posteriori estimated state vector at time k-1, P (k | k-1) is the covariance at time k estimated from time k-1, P (k-1| k-1) is the a posteriori estimated covariance at time k-1, Q is the process noise covariance;
step 1.3, update correction
Firstly, calculating Kalman gain, then calculating posterior estimation by using the prior estimation in the step 1.2, and simultaneously updating the prior error to the posterior error:
K(k)＝P(k|k-1)HT[HP(k|k-1)HT+R]-1 (5)，
Figure BDA0002771048300000034
P(k|k)＝(I-K(k)H)P(k|k-1) (7)，
in the formulae (5), (6) and (7),
Figure BDA0002771048300000041
the state vector at the time of updating k is predicted from the estimated state vector, and P (k | k) is predicted from the estimated state vectorCovariance at time k, I is the identity matrix, k (k) is the kalman gain at time k, R is the measurement noise covariance;
step 1.4, establishing regression equations of R and Q by adopting a least square support vector machine regression algorithm, and processing process noise and measurement noise;
and step 1.5, substituting the regression equation of the process noise covariance Q and the measurement noise covariance R in the step 1.4 into the step 1.2 and the step 1.3 to obtain an improved Kalman vibration filtering algorithm, and filtering the tremor signal of the hand to obtain a target track signal.
The step 1.4 is specifically as follows:
step 1.4.1, establishing a least square support vector machine training set of
Figure BDA0002771048300000042
Wherein Si∈RnIs a mixture of the measured vibration signal and the ideal control signal, as an input vector, RiE, R is the covariance of the measurement noise of the vibration signal in the mixed signal;
step 1.4.2, according to the least square support vector machine theory, converting the regression problem of the nonlinear function of R and Q into the regression problem of the linear function, mapping variables R and Q into a high-dimensional space F, and obtaining the nonlinear function:
R(k)＝ωTΦ(s)+bω∈F，b∈R (8)，
in the formula (8), phi(s) is a nonlinear mapping function, and omega and b are undetermined parameters;
step 1.4.3, the optimization problem of the least squares support vector machine is defined according to step 1.4.2:
Figure BDA0002771048300000043
Ri＝ωTΦ(Si)+b+ei i＝1，...，N (10)，
in formulae (9) and (10), eiIs the relaxation variable at time i, C is a constant greater than 0, and equation (10) is the constraint bar of equation (9)A member;
step 1.4.4, list lagrangian functions according to step 1.4.3:
Figure BDA0002771048300000051
in the formula (11), αiE, R, i 1, N, is a lagrange parameter;
step 1.4.5, for the general optimization problem with equality constraint in step 1.4.3, KKT gives the necessary condition for judging whether the solution value is the optimal solution, and calculates the partial derivative for all parameters of the lagrangian function listed in step 1.4.4;
step 1.4.6, set forth the system of linear equations for step 1.4.5:
Figure BDA0002771048300000052
in the formula (12), the reaction mixture is,
Figure BDA0002771048300000053
R＝[R1，...，RN]T，α＝[α1，...，αN]TΩ is the kernel matrix;
Ωij＝φ(si)Tφ(sj)＝K(si，sj) i，j＝1，...，N (13)，
in formula (13), K(s)i，sj) Is a kernel function, ΩijIs a kernel matrix of i rows and j columns;
step 1.4.7, solving the formula (12) to obtain the coefficient of the regression equation, namely obtaining the regression equation of the least square support vector machine for measuring the noise covariance R:
Figure BDA0002771048300000054
step 1.4.8, establishing a regression equation of the process noise covariance Q using steps 1.4.5 and 1.4.6, specifically:
Figure BDA0002771048300000061
in formulae (14) and (15) < alpha'iIs relative to aiLagrange parameter of (g), K(s)i，sj) Is relative to K(s)i，sj) B' is the pending parameter relative to b.
The step 2 specifically comprises the following steps:
step 2.1, carrying out track planning on the target track signal in a task space by a linear track planning method;
step 2.2, carrying out trajectory planning on the target trajectory signal in the joint space by a Chebyshev pseudo-spectrum method;
and 2.3, performing time-smoothness optimization on the tracks planned in the step 2.1 and the step 2.2 by a quadratic programming method to obtain an actual track signal.
The step 2.1 specifically comprises the following steps:
firstly, the position vector and the attitude vector of the mobile mechanical arm terminal point motion at the starting point and the terminal point of a task space are respectively represented as:
P1＝[x1，y1，z1]T、P2＝[x2，y2，z2]T、
Figure BDA0002771048300000062
Figure BDA0002771048300000063
the position vector and the attitude vector connecting the initial point and the target point are respectively:
P12＝P2-P1＝(x1-x2，y1-y2，z1-z2)T (16)，
Figure BDA0002771048300000064
in the formulae (16) and (17), [ x, y, z]And
Figure BDA0002771048300000065
representing a position vector and a posture vector of a terminal point of a mobile mechanical arm under a task space coordinate system;
assuming that it takes time T from the initial value movement to the target value of the end point of the mobile manipulator, and then discretizing the position vector and the attitude vector, the position vector and the attitude vector of the end point of the mobile manipulator at the time T can be calculated:
Figure BDA0002771048300000071
Figure BDA0002771048300000072
in the formulae (18), (19), PtAnd RtFor the position vector and attitude vector at time t,
Figure BDA0002771048300000073
Figure BDA0002771048300000074
t denotes the time, te (t)1，t2) V and ω represent the average linear velocity and the average angular velocity from the initial value to the target value at the end point of the hand arm, respectively.
The step 2.2 specifically comprises the following steps:
step 2.2.1, time intervals are changed
The Chebyshev pseudo-spectrum method is a track approximately approximating state and control based on interpolation polynomial, wherein Chebyshev-Gauss-Lobatto points are selected as nodes and defined in an interval of [ -1, 1]Introducing a new time variable tau epsilon-1, 1]The original time interval [ t ]0，tf]Conversion to [ -1, 1 [ ]]And t is expressed as:
Figure BDA0002771048300000075
step 2.2.2 calculation of discrete nodes
Chebyshev polynomial T selected from discrete nodes by Chebyshev pseudo-spectrum methodN(t)＝cos(Ncos-1(t)) the extreme point, i.e. the CGL point;
step 2.2.3 interpolation approximation of state variables and control variables
Firstly, taking N +1 discrete points, and constructing a Lagrange interpolation polynomial as an approximate value of a continuous state and a control variable, wherein an approximate expression of the continuous state x (tau) and the control variable u (tau) is as follows:
Figure BDA0002771048300000081
Figure BDA0002771048300000082
in the formulae (21) and (22), xjIs the state at time j, ujIs the control variable at time j, wherejIs the lagrange interpolation basis function;
step 2.2.4, processing of dynamic constraints
The derivative of the formula (21) gives a continuous state at τkDerivative at point
Figure BDA0002771048300000083
The approximate expression is:
Figure BDA0002771048300000084
in the formula (23), DkjIs (N +1) × (N +1) differential matrix D row k, column j element;
obtained by formula (23)
Figure BDA0002771048300000085
Instead of the state variable derivative with respect to time and discretizing at the interpolation nodes, the differential equation constraints of the original optimal control problem can be transformed into algebraic constraints, i.e. for k 0, 1.
Figure BDA0002771048300000086
In the formula (24), x (τ)k) Time continuous state, u (τ)k) Is a control variable, τkIs the CGL point, τfAnd τ0Is a new time variable;
step 2.2.5, approximate calculation of performance index integrals
The Clenshaw-Curtis numerical integration is adopted to carry out approximate calculation on the integral term in the optimized performance index, and for any continuous function p (t) in the range of [ -1, 1], the integral can be approximately calculated by adopting the accumulation of function values at N +1 CGL discrete points, namely
Figure BDA0002771048300000091
In the formula (25), ωk(k ═ 0, 1.., N) is the Clenshaw-Curtis integration weight.
The step 2.3 is specifically as follows:
step 2.3.1, establishing constraint conditions of optimization model
Firstly, in order to operate safety, a position of a joint of a slave manipulator arm is generally limited by a stopper, and in order to avoid exceeding a joint movement range, a joint track is subjected to position constraint, which is specifically expressed as follows:
θmin≤θ(t)≤θmax (26)，
in the formula (26), θ (t) and θminAnd thetamaxThe minimum value and the maximum value of the joint angle and the joint rotatable angle are respectively;
because the motor and the reducer have certain limits on the input rotating speed, the speed of the joint track is restricted as follows:
Figure BDA0002771048300000092
Figure BDA0002771048300000093
in the formulae (27) and (28),
Figure BDA0002771048300000094
seed of a plant
Figure BDA0002771048300000095
The angular velocity and the maximum value of the angular velocity of the joint are respectively;
Figure BDA0002771048300000096
and
Figure BDA0002771048300000097
the joint angular acceleration and the maximum angular acceleration are respectively;
the surgical robot joint is driven by a motor, the motor has certain limitation on the maximum output torque, and the output torque of the motor is constrained as follows through a dynamic model:
Figure BDA0002771048300000101
in the formula (29), θ (t),
Figure BDA0002771048300000102
And
Figure BDA0002771048300000103
respectively motor angle, angular velocity and angular acceleration, JeqAnd BeqRespectively equivalent inertia and equivalent damping of the joint, M is a reduction ratio coefficient, G (theta (t)) is joint connecting rod gravity, and tau (t) and taumaxRespectively the output torque and the rated torque of the motor;
step 2.3.2, adopting time optimal and smoothness optimal multi-objective optimization, and obtaining the expression of the sequence quadratic programming constraint of the optimization model from formulas (26) to (29) as follows:
Figure BDA0002771048300000104
Figure BDA0002771048300000105
in the formulae (30) and (31), hi＝ti+1-ti，kJIs the weight coefficient;
and optimizing the planned track through the sequential quadratic programming constraint of the optimization model.
The step 3 specifically comprises the following steps:
step 3.1, determining a master-slave mapping function;
and 3.2, determining an operation proportional coefficient, introducing the operation proportional coefficient into the master-slave mapping function in the step 3.1, and mapping the motion poses of the master hand and the slave hand by taking the actual track signal as input to realize the predictive control of the track of the master-slave surgical robot.
The step 3.1 is specifically as follows:
step 3.1.1, establishing a master hand and slave hand space coordinate system;
step 3.1.2, establishing a master-slave mapping function;
the pose of the main hand operation coordinate system E-XYZ and the display coordinate system D-XYZ is expressed as follows:
Figure BDA0002771048300000111
in the formula (32), the compound represented by the formula (32),
Figure BDA0002771048300000112
a transformation matrix representing the master hand operation coordinate system M-XYZ relative to the display coordinate system D-XYZ,
Figure BDA0002771048300000113
represents a principalA transformation matrix of the hand end coordinate system H-XYZ relative to the main hand operation table coordinate system M-XYZ,
Figure BDA0002771048300000114
a transformation matrix representing the slave operation end coordinate system S-XYZ relative to the slave hand end coordinate system E-XYZ,
Figure BDA0002771048300000115
a transformation matrix representing a slave hand coordinate system T-XYZ relative to a slave operation end coordinate system S-XYZ; therefore, the method comprises the following steps:
Figure BDA0002771048300000116
and (3) carrying out a Cartesian space increment mapping strategy on the formula (33) to obtain a master-slave mapping function:
Figure BDA0002771048300000117
in the formula (34), Ψ represents a control period,
Figure BDA0002771048300000118
is a matrix of positions from the hand(s),
Figure BDA0002771048300000119
is a matrix of the poses of the slave hand,
Figure BDA00027710483000001110
is a matrix of the position of the master hand,
Figure BDA00027710483000001111
is the pose matrix of the master hand.
The step 3.2 is specifically as follows:
setting λ as a running scaling factor in the master-slave mapping function, and λ only acts on position control, defining λ as:
Figure BDA00027710483000001112
in equation (35), σ is a position magnification factor that determines the size of the hand-made space, and can be obtained by:
Figure BDA0002771048300000121
in the formula (36), DMAnd HMDiameter and height of the working space of the master hand, respectively, DSAnd HSσ is determined from the hand-made space diameter and height, respectively, and from the hand-tip vibration amplitude:
Figure BDA0002771048300000122
in the formula (37), C is the amplitude of vibration from the end of the hand, CΔIs the minimum vibration amplitude of the slave hand in the step signal response;
by introducing operation ratio control in the formula (34), a master-slave pose mapping function can be obtained as follows:
Figure BDA0002771048300000123
and controlling the optimized track through a master-slave pose mapping function, and realizing the prediction control of the track of the master-slave surgical robot.
The invention has the beneficial effects that:
the invention relates to a master-slave mode surgical robot track prediction control method, which is characterized in that a least square support vector machine Kalman filtering algorithm is designed to filter the slight vibration of a surgical robot, so that the filtering time delay can be controlled within a real-time surgery acceptable range, and the filtering precision is ensured; the invention discloses a trajectory prediction control method of a master-slave surgical robot, which provides a Chebyshev pseudo-spectrum method and a sequence quadratic programming for performing trajectory planning prediction on the tail end of a slave mobile manipulator arm, introduces an operation control proportion, reduces the amplitude of vibration of the tail end of a slave hand and simultaneously achieves the aim of accurate operation; the invention relates to a master-slave mode surgical robot trajectory prediction control method, which is based on a Chebyshev pseudo-spectrum method, a sequence quadratic programming method and a Kalman filtering (Kalman filtering) algorithm to carry out slave hand trajectory prediction planning and filtering shake elimination; the Chebyshev pseudo-spectral method can convert the track optimization into a nonlinear programming problem, the discrete numerical solution of the Chebyshev pseudo-spectral method can be consistently close to the optimal solution of the original problem, and the Chebyshev pseudo-spectral method has quite high accuracy; kalman filtering is an optimal prediction algorithm, and can be combined with a state equation and input and output data of a system to perform optimal prediction, so that the terminal vibration filtering rate is improved, and the time delay in the operation process is overcome.
Drawings
FIG. 1 is a flow chart of a master-slave surgical robot trajectory prediction control method of the present invention;
FIG. 2 is a coordinate system of a master-slave pose mapping function established by the trajectory prediction control method of a master-slave surgical robot according to the present invention;
fig. 3 is a trace signal comparison graph of a hand tremor signal before and after filtering.
Detailed Description
The present invention will be described in detail below with reference to the accompanying drawings and specific embodiments.
The invention discloses a master-slave mode surgical robot track prediction control method, which is implemented according to the following steps as shown in figure 1:
step 1, designing a least square support vector machine Kalman vibration filtering algorithm, and filtering tremor signals of hands to obtain target track signals;
step 2, respectively carrying out trajectory planning on the target trajectory signal by a linear trajectory planning method and a Chebyshev pseudo-spectrum method to obtain a planned trajectory, and optimizing the planned trajectory by a quadratic planning method to obtain an actual trajectory signal;
and 3, mapping the motion poses of the master hand and the slave hand by taking the actual track signal as input through a master-slave pose mapping function, and realizing the prediction control of the track of the master-slave surgical robot.
The step 1 specifically comprises the following steps:
step 1.1, collecting the running track of a master hand, carrying out discretization calibration on the running track, and writing a Kalman filtering process into a state equation and a measurement process:
and (3) state process:
X(k)＝AX(k-1)+w(k) (1)，
the measurement process comprises the following steps:
Z(k)＝HX(k)+v(k) (2)，
in the formulas (1) and (2), x (k) is the state of the vibration signal in the main hand, the matrix a represents the state change, the vector z (k) is the signal measurement result, the matrix H represents the measurement change, the vector w (k) is the state noise, and the vector v (k) is the measurement noise, wherein w (k) and v (k) are gaussian white noises with the average value of 0, which are assumed to be independent of each other;
step 1.2, prediction
Predicting the value of the next time state according to the estimation value of the previous time state to become prior estimation, and predicting the error of the next time state to become prior error:
Figure BDA0002771048300000141
P(k|k-1)＝AP(k-1|k-1)AT+Q (4)，
in the formulas (3) and (4),
Figure BDA0002771048300000142
the state vector at time k is estimated from time k-1,
Figure BDA0002771048300000143
is the a posteriori estimated state vector at time k-1, P (k | k-1) is the covariance at time k estimated from time k-1, P (k-1| k-1) is the a posteriori estimated covariance at time k-1, Q is the process noise covariance;
step 1.3, update correction
Firstly, calculating Kalman gain, then calculating posterior estimation by using the prior estimation in the step 1.2, and simultaneously updating the prior error to the posterior error:
K(k)＝P(k|k-1)HT[HP(k|k-1)HT+R]-1 (5)，
Figure BDA0002771048300000151
P(k|k)＝(I-K(k)H)P(k|k-1) (7)，
in the formulae (5), (6) and (7),
Figure BDA0002771048300000152
predicting a state vector at the time k of updating according to the estimated state vector, predicting covariance at the time k of updating according to the estimated state vector by P (k | k), wherein I is an identity matrix, K (k) is Kalman gain at the time k, and R is measurement noise covariance;
step 1.4, establishing regression equations of R and Q by adopting a least square support vector machine regression algorithm, and processing process noise and measurement noise;
and step 1.5, substituting the regression equation of the process noise covariance Q and the measurement noise covariance R in the step 1.4 into the step 1.2 and the step 1.3 to obtain an improved Kalman vibration filtering algorithm, and filtering the tremor signal of the hand to obtain a target track signal.
The step 1.4 is specifically as follows:
step 1.4.1, establishing a least square support vector machine training set of
Figure BDA0002771048300000153
Wherein Si∈RnIs a mixture of the measured vibration signal and the ideal control signal, as an input vector, RiE, R is the covariance of the measurement noise of the vibration signal in the mixed signal;
step 1.4.2, according to the least square support vector machine theory, converting the regression problem of the nonlinear function of R and Q into the regression problem of the linear function, mapping variables R and Q into a high-dimensional space F, and obtaining the nonlinear function:
R(k)＝ωTφ(s)+b ω∈F，b∈R (8)，
in the formula (8), phi(s) is a nonlinear mapping function, and omega and b are undetermined parameters;
step 1.4.3, the optimization problem of the least squares support vector machine is defined according to step 1.4.2:
Figure BDA0002771048300000161
Ri＝ωTΦ(Si)+b+ei i＝1，...，N (10)，
in formulae (9) and (10), eiIs the relaxation variable at time i, C is a constant greater than 0, and equation (10) is the constraint of equation (9);
step 1.4.4, list lagrangian functions according to step 1.4.3:
Figure BDA0002771048300000162
in the formula (11), αiE, R, i 1, N, is a lagrange parameter;
step 1.4.5, for the general optimization problem with equality constraint in step 1.4.3, KKT gives the necessary condition for judging whether the solution value is the optimal solution, and calculates the partial derivative for all parameters of the lagrangian function listed in step 1.4.4;
step 1.4.6, set forth the system of linear equations for step 1.4.5:
Figure BDA0002771048300000163
in the formula (12), the reaction mixture is,
Figure BDA0002771048300000164
R＝[R1，...，RN]T，α＝[α1，...，αN]TΩ is the kernel matrix;
Ωij＝Φ(si)TΦ(sj)＝K(si，sj) i，j＝1，...，N (13)，
formula (A), (B) and13) in, K(s)i，sj) Is a kernel function, ΩijIs a kernel matrix of i rows and j columns;
step 1.4.7, solving the formula (12) to obtain the coefficient of the regression equation, namely obtaining the regression equation of the least square support vector machine for measuring the noise covariance R:
Figure BDA0002771048300000171
step 1.4.8, establishing a regression equation of the process noise covariance Q using steps 1.4.5 and 1.4.6, specifically:
Figure BDA0002771048300000172
in formulae (14) and (15) < alpha'iIs relative to aiLagrange parameter of (g), K(s)i，sj) Is relative to K(s)i，sj) B' is the pending parameter relative to b.
The step 2 specifically comprises the following steps:
step 2.1, carrying out track planning on the target track signal in a task space by a linear track planning method;
step 2.2, carrying out trajectory planning on the target trajectory signal in the joint space by a Chebyshev pseudo-spectrum method;
and 2.3, performing time-smoothness optimization on the tracks planned in the step 2.1 and the step 2.2 by a quadratic programming method to obtain an actual track signal.
The step 2.1 specifically comprises the following steps:
firstly, the position vector and the attitude vector of the mobile mechanical arm terminal point motion at the starting point and the terminal point of a task space are respectively represented as:
P1＝[x1，y1，z1]T、P2＝[x2，y2，z2]T、
Figure BDA0002771048300000173
Figure BDA0002771048300000174
the position vector and the attitude vector connecting the initial point and the target point are respectively:
P12＝P2-P1＝(x1-x2，y1-y2，z1-z2)T (16)，
Figure BDA0002771048300000181
in the formulae (16) and (17), [ x, y, z]And
Figure BDA0002771048300000182
representing a position vector and a posture vector of a terminal point of a mobile mechanical arm under a task space coordinate system;
assuming that it takes time T from the initial value movement to the target value of the end point of the mobile manipulator, and then discretizing the position vector and the attitude vector, the position vector and the attitude vector of the end point of the mobile manipulator at the time T can be calculated:
Figure BDA0002771048300000183
Figure BDA0002771048300000184
in the formulae (18), (19), PtAnd RtFor the position vector and attitude vector at time t,
Figure BDA0002771048300000185
Figure BDA0002771048300000186
t denotes the time, te (t)1，t2) And v and ω denote slave mobile machines, respectivelyThe average linear velocity and the average angular velocity of the arm end point from the initial value to the target value.
The step 2.2 specifically comprises the following steps:
step 2.2.1, time intervals are changed
The Chebyshev pseudo-spectrum method is a track approximately approximating state and control based on interpolation polynomial, wherein Chebyshev-Gauss-Lobatto points are selected as nodes and defined in an interval of [ -1, 1]Introducing a new time variable tau epsilon-1, 1]The original time interval [ t ]0，tf]Conversion to [ -1, 1 [ ]]And t is expressed as:
Figure BDA0002771048300000187
step 2.2.2 calculation of discrete nodes
Chebyshev polynomial T selected from discrete nodes by Chebyshev pseudo-spectrum methodN(t)＝cos(Ncos-1(t)) the extreme point, i.e. the CGL point;
step 2.2.3 interpolation approximation of state variables and control variables
Firstly, taking N +1 discrete points, and constructing a Lagrange interpolation polynomial as an approximate value of a continuous state and a control variable, wherein an approximate expression of the continuous state x (tau) and the control variable u (tau) is as follows:
Figure BDA0002771048300000191
Figure BDA0002771048300000192
in the formulae (21) and (22), xjIs the state at time j, ujIs the control variable at time j, wherejIs the lagrange interpolation basis function;
step 2.2.4, processing of dynamic constraints
The derivative of the formula (21) gives a continuous state at τkDerivative at point
Figure BDA0002771048300000193
The approximate expression is:
Figure BDA0002771048300000194
in the formula (23), DkjIs (N +1) × (N +1) differential matrix D row k, column j element;
obtained by formula (23)
Figure BDA0002771048300000195
Instead of the state variable derivative with respect to time and discretizing at the interpolation nodes, the differential equation constraints of the original optimal control problem can be transformed into algebraic constraints, i.e. for k 0, 1.
Figure BDA0002771048300000201
In the formula (24), x (τ)k) Time continuous state, u (τ)k) Is a control variable, τkIs the CGL point, τfAnd τ0Is a new time variable;
step 2.2.5, approximate calculation of performance index integrals
The Clenshaw-Curtis numerical integration is adopted to carry out approximate calculation on the integral term in the optimized performance index, and for any continuous function p (t) in the range of [ -1, 1], the integral can be approximately calculated by adopting the accumulation of function values at N +1 CGL discrete points, namely
Figure BDA0002771048300000202
In the formula (25), ωk(k ═ 0, 1.., N) is the Clenshaw-Curtis integration weight.
The step 2.3 is specifically as follows:
step 2.3.1, establishing constraint conditions of optimization model
Firstly, in order to operate safety, a position of a joint of a slave manipulator arm is generally limited by a stopper, and in order to avoid exceeding a joint movement range, a joint track is subjected to position constraint, which is specifically expressed as follows:
θmin≤θ(t)≤θmax (26)，
in the formula (26), θ (t) and θminAnd thetamaxThe minimum value and the maximum value of the joint angle and the joint rotatable angle are respectively;
because the motor and the reducer have certain limits on the input rotating speed, the speed of the joint track is restricted as follows:
Figure BDA0002771048300000203
Figure BDA0002771048300000204
in the formulae (27) and (28),
Figure BDA0002771048300000211
and
Figure BDA0002771048300000212
the angular velocity and the maximum value of the angular velocity of the joint are respectively;
Figure BDA0002771048300000213
and
Figure BDA0002771048300000214
the joint angular acceleration and the maximum angular acceleration are respectively;
the surgical robot joint is driven by a motor, the motor has certain limitation on the maximum output torque, and the output torque of the motor is constrained as follows through a dynamic model:
Figure BDA0002771048300000215
in the formula (29), θ (t),
Figure BDA0002771048300000216
And
Figure BDA0002771048300000217
respectively motor angle, angular velocity and angular acceleration, JeqAnd BeqRespectively equivalent inertia and equivalent damping of the joint, M is a reduction ratio coefficient, G (theta (t)) is joint connecting rod gravity, and tau (t) and taumaxRespectively the output torque and the rated torque of the motor;
step 2.3.2, adopting time optimal and smoothness optimal multi-objective optimization, and obtaining the expression of the sequence quadratic programming constraint of the optimization model from formulas (26) to (29) as follows:
Figure BDA0002771048300000218
Figure BDA0002771048300000219
in the formulae (30) and (31), hi＝ti+1-ti，kJIs the weight coefficient;
and optimizing the planned track through the sequential quadratic programming constraint of the optimization model.
The step 3 specifically comprises the following steps:
step 3.1, determining a master-slave mapping function;
and 3.2, determining an operation proportional coefficient, introducing the operation proportional coefficient into the master-slave mapping function in the step 3.1, and mapping the motion poses of the master hand and the slave hand by taking the actual track signal as input to realize the predictive control of the track of the master-slave surgical robot.
The step 3.1 is specifically as follows:
step 3.1.1, establishing a master hand and slave hand space coordinate system; as shown in fig. 2;
step 3.1.2, establishing a master-slave mapping function;
the pose of the main hand operation coordinate system E-XYZ and the display coordinate system D-XYZ is expressed as follows:
Figure BDA0002771048300000221
in the formula (32), the compound represented by the formula (32),
Figure BDA0002771048300000222
a transformation matrix representing the master hand operation coordinate system M-XYZ relative to the display coordinate system D-XYZ,
Figure BDA0002771048300000223
a transformation matrix representing the coordinate system H-XYZ of the end of the master hand relative to the coordinate system M-XYZ of the master hand operation table,
Figure BDA0002771048300000224
a transformation matrix representing the slave operation end coordinate system S-XYZ relative to the slave hand end coordinate system E-XYZ,
Figure BDA0002771048300000225
a transformation matrix representing a slave hand coordinate system T-XYZ relative to a slave operation end coordinate system S-XYZ; therefore, the method comprises the following steps:
Figure BDA0002771048300000226
and (3) carrying out a Cartesian space increment mapping strategy on the formula (33) to obtain a master-slave mapping function:
Figure BDA0002771048300000227
in the formula (34), Ψ represents a control period,
Figure BDA0002771048300000228
is a matrix of positions from the hand(s),
Figure BDA0002771048300000229
is a matrix of the poses of the slave hand,
Figure BDA00027710483000002210
is a matrix of the position of the master hand,
Figure BDA00027710483000002211
is the pose matrix of the master hand.
The step 3.2 is specifically as follows:
setting λ as a running scaling factor in the master-slave mapping function, and λ only acts on position control, defining λ as:
Figure BDA0002771048300000231
in equation (35), σ is a position magnification factor that determines the size of the hand-made space, and can be obtained by:
Figure BDA0002771048300000232
in the formula (36), DMAnd HMDiameter and height of the working space of the master hand, respectively, DSAnd HSσ is determined from the hand-made space diameter and height, respectively, and from the hand-tip vibration amplitude:
Figure BDA0002771048300000233
in the formula (37), C is the amplitude of vibration from the end of the hand, CΔIs the minimum vibration amplitude of the slave hand in the step signal response;
by introducing operation ratio control in the formula (34), a master-slave pose mapping function can be obtained as follows:
Figure BDA0002771048300000234
and controlling the optimized track through a master-slave pose mapping function, and realizing the prediction control of the track of the master-slave surgical robot.
As can be seen from fig. 3, the track signal before filtering becomes very unsmooth due to the inclusion of the hand tremor signal, which may cause the vibration of the surgical instrument from the end of the hand, affecting the precision and quality of the surgery; after the filtering is carried out by the least square support vector machine Kalman filter designed in the step 1, the tremble part of the mixed control signal can be effectively filtered, and the coincidence degree of the two signals is very high. The filtering algorithm designed by the invention has higher precision and no obvious time delay, and meets the requirement of real-time operation.

Claims (10)

1. A master-slave mode surgical robot track prediction control method is characterized by comprising the following steps:
step 1, designing a least square support vector machine Kalman vibration filtering algorithm, and filtering tremor signals of hands to obtain target track signals;
step 2, respectively carrying out trajectory planning on the target trajectory signal through a linear trajectory planning method and a Chebyshev pseudo-spectrum method to obtain a planned trajectory, and optimizing the planned trajectory through a quadratic planning method to obtain an actual trajectory signal;
and 3, mapping the motion poses of the master hand and the slave hand by taking the actual track signal as input through a master-slave pose mapping function, and realizing the prediction control of the track of the master-slave surgical robot.
2. The method for trajectory prediction control of a master-slave surgical robot according to claim 1, wherein the step 1 is specifically:
step 1.1, collecting the running track of a master hand, carrying out discretization calibration on the running track, and writing a Kalman filtering process into a state equation and a measurement process:
and (3) state process:
X(k)＝AX(k-1)+w(k) (1)，
the measurement process comprises the following steps:
Z(k)＝HX(k)+v(k) (2)，
in the formulas (1) and (2), x (k) is the state of the vibration signal in the main hand, the matrix a represents the state change, the vector z (k) is the signal measurement result, the matrix H represents the measurement change, the vector w (k) is the state noise, and the vector v (k) is the measurement noise, wherein w (k) and v (k) are gaussian white noises with the average value of 0, which are assumed to be independent of each other;
step 1.2, prediction
Predicting the value of the next time state according to the estimation value of the previous time state to become prior estimation, and predicting the error of the next time state to become prior error:
Figure FDA0002771048290000021
P(k|k-1)＝AP(k-1|k-1)AT+Q (4)，
in the formulas (3) and (4),
Figure FDA0002771048290000022
the state vector at time k is estimated from time k-1,
Figure FDA0002771048290000023
is the a posteriori estimated state vector at time k-1, P (k | k-1) is the covariance at time k estimated from time k-1, P (k-1| k-1) is the a posteriori estimated covariance at time k-1, Q is the process noise covariance;
step 1.3, update correction
Firstly, calculating Kalman gain, then calculating posterior estimation by using the prior estimation in the step 1.2, and simultaneously updating the prior error to the posterior error:
K(k)＝P(k|k-1)HT[HP(k|k-1)HT+R]-1 (5)，
Figure FDA0002771048290000024
P(k|k)＝(I-K(k)H)P(k|k-1) (7)，
in the formulae (5), (6) and (7),
Figure FDA0002771048290000025
predicting a state vector at the time k of updating according to the estimated state vector, predicting covariance at the time k of updating according to the estimated state vector by P (k | k), wherein I is an identity matrix, K (k) is Kalman gain at the time k, and R is measurement noise covariance;
step 1.4, establishing regression equations of R and Q by adopting a least square support vector machine regression algorithm, and processing process noise and measurement noise;
and step 1.5, substituting the regression equation of the process noise covariance Q and the measurement noise covariance R in the step 1.4 into the step 1.2 and the step 1.3 to obtain an improved Kalman vibration filtering algorithm, and filtering the tremor signal of the hand to obtain a target track signal.
3. The master-slave surgical robot trajectory prediction control method according to claim 2, wherein the step 1.4 is specifically:
step 1.4.1, establishing a least square support vector machine training set of
Figure FDA0002771048290000031
Wherein Si∈RnThe signal is a mixed signal of a vibration signal and an ideal control signal obtained by measurement, the signal is an input vector, Ri belongs to R, and the signal is a measurement noise covariance of the vibration signal in the mixed signal;
step 1.4.2, according to the least square support vector machine theory, converting the regression problem of the nonlinear function of R and Q into the regression problem of the linear function, mapping variables R and Q into a high-dimensional space F, and obtaining the nonlinear function:
R(k)＝ωTφ(s)+b ω∈F，b∈R (8)，
in the formula (8), phi(s) is a nonlinear mapping function, and omega and b are undetermined parameters;
step 1.4.3, the optimization problem of the least squares support vector machine is defined according to step 1.4.2:
Figure FDA0002771048290000032
Ri＝ωTφ(Si)+b+ei i＝1，...，N (10)，
in formulae (9) and (10), eiIs the relaxation variable at time i, C is a constant greater than 0, and equation (10) is the constraint of equation (9);
step 1.4.4, list lagrangian functions according to step 1.4.3:
Figure FDA0002771048290000041
in the formula (11), αiE, R, i 1, N, is a lagrange parameter;
step 1.4.5, for the general optimization problem with equality constraint in step 1.4.3, KKT gives the necessary condition for judging whether the solution value is the optimal solution, and calculates the partial derivative for all parameters of the lagrangian function listed in step 1.4.4;
step 1.4.6, set forth the system of linear equations for step 1.4.5:
Figure FDA0002771048290000042
in the formula (12), the reaction mixture is,
Figure FDA0002771048290000043
R＝[R1，...，RN]T，α＝[α1，...，αN]TΩ is the kernel matrix;
Ωij＝φ(si)TΦ(sj)＝K(si，sj) i，j＝1，...，N (13)，
in formula (13), K(s)i，sj) Is a kernel function, ΩijIs a kernel matrix of i rows and j columns;
step 1.4.7, solving the formula (12) to obtain the coefficient of the regression equation, namely obtaining the regression equation of the least square support vector machine for measuring the noise covariance R:
Figure FDA0002771048290000044
step 1.4.8, establishing a regression equation of the process noise covariance Q using steps 1.4.5 and 1.4.6, specifically:
Figure FDA0002771048290000045
in formulae (14) and (15) < alpha'iIs relative to aiLagrange parameter of (g), K(s)i，sj) Is relative to K(s)i，sj) B' is the pending parameter relative to b.
4. The master-slave surgical robot trajectory prediction control method according to claim 2, wherein step 2 is specifically:
step 2.1, carrying out track planning on the target track signal in a task space by a linear track planning method;
step 2.2, carrying out trajectory planning on the target trajectory signal in a joint space by a Chebyshev pseudo-spectrum method;
and 2.3, performing time-smoothness optimization on the tracks planned in the step 2.1 and the step 2.2 by a quadratic programming method to obtain an actual track signal.
5. The master-slave surgical robot trajectory prediction control method according to claim 4, wherein step 2.1 is specifically:
firstly, the position vector and the attitude vector of the mobile mechanical arm terminal point motion at the starting point and the terminal point of a task space are respectively represented as:
P1＝[x1，y1，z1]T、P2＝[x2，y2，z2]T、
Figure FDA0002771048290000051
then, the position vector and the attitude vector connecting the initial point and the target point are respectively:
P12＝P2-P1＝(x1-x2，y1-y2，z1-z2)T (16)，
Figure FDA0002771048290000052
in the formulae (16) and (17), [ x, y, z]And
Figure FDA0002771048290000053
representing a position vector and a posture vector of a terminal point of a mobile mechanical arm under a task space coordinate system;
assuming that it takes time T from the initial value movement to the target value of the end point of the mobile manipulator, and then discretizing the position vector and the attitude vector, the position vector and the attitude vector of the end point of the mobile manipulator at the time T can be calculated:
Figure FDA0002771048290000061
Figure FDA0002771048290000062
in the formulae (18), (19), PtAnd RtFor the position vector and attitude vector at time t,
Figure FDA0002771048290000063
Figure FDA0002771048290000064
t denotes the time, te (t)1，t2) V and ω represent the average linear velocity and the average angular velocity from the initial value to the target value at the end point of the hand arm, respectively.
6. The master-slave surgical robot trajectory prediction control method according to claim 5, wherein the step 2.2 is specifically:
step 2.2.1, time intervals are changed
The Chebyshev pseudo-spectrum method is a track approximately approximating state and control based on interpolation polynomial, wherein Chebyshev-Gauss-Lobatto points are selected as nodes and defined in an interval of [ -1, 1]Introducing a new time variable tau epsilon-1, 1]The original time interval [ t ]0，tf]Conversion to [ -1, 1 [ ]]And t is expressed as:
Figure FDA0002771048290000065
step 2.2.2 calculation of discrete nodes
Chebyshev polynomial T selected from discrete nodes by Chebyshev pseudo-spectrum methodN(t)＝cos(Ncos-1(t)) the extreme point, i.e. the CGL point;
step 2.2.3 interpolation approximation of state variables and control variables
Firstly, taking N +1 discrete points, and constructing a Lagrange interpolation polynomial as an approximate value of a continuous state and a control variable, wherein an approximate expression of the continuous state x (tau) and the control variable u (tau) is as follows:
Figure FDA0002771048290000071
Figure FDA0002771048290000072
in the formulae (21) and (22), xjIs the shape of time jState ujIs the control variable at time j, wherejIs the lagrange interpolation basis function;
step 2.2.4, processing of dynamic constraints
The derivative of the formula (21) gives a continuous state at τkDerivative at point
Figure FDA0002771048290000073
The approximate expression is:
Figure FDA0002771048290000074
in the formula (23), DkjIs (N +1) × (N +1) differential matrix D row k, column j element;
obtained by formula (23)
Figure FDA0002771048290000075
Instead of the state variable derivative with respect to time and discretizing at the interpolation nodes, the differential equation constraints of the original optimal control problem can be transformed into algebraic constraints, i.e. for k 0, 1.
Figure FDA0002771048290000076
In the formula (24), x (τ)k) Time continuous state, u (τ)k) Is a control variable, τkIs the CGL point, τfAnd τ0Is a new time variable;
step 2.2.5, approximate calculation of performance index integrals
The Clenshaw-Curtis numerical integration is adopted to carry out approximate calculation on the integral term in the optimized performance index, and for any continuous function p (t) in the range of [ -1, 1], the integral can be approximately calculated by adopting the accumulation of function values at N +1 CGL discrete points, namely
Figure FDA0002771048290000081
In the formula (25), ωk(k ═ 0, 1.., N) is the Clenshaw-Curtis integration weight.
7. The master-slave surgical robot trajectory prediction control method according to claim 6, wherein the step 2.3 is specifically:
step 2.3.1, establishing constraint conditions of optimization model
Firstly, in order to operate safety, a position of a joint of a slave manipulator arm is generally limited by a stopper, and in order to avoid exceeding a joint movement range, a joint track is subjected to position constraint, which is specifically expressed as follows:
θmin≤θ(t)≤θmax (26)，
in the formula (26), θ (t) and θminAnd thetamaxThe minimum value and the maximum value of the joint angle and the joint rotatable angle are respectively;
because the motor and the reducer have certain limits on the input rotating speed, the speed of the joint track is restricted as follows:
Figure FDA0002771048290000082
Figure FDA0002771048290000083
in the formulae (27) and (28),
Figure FDA0002771048290000084
and
Figure FDA0002771048290000085
the angular velocity and the maximum value of the angular velocity of the joint are respectively;
Figure FDA0002771048290000086
and
Figure FDA0002771048290000087
the joint angular acceleration and the maximum angular acceleration are respectively;
the surgical robot joint is driven by a motor, the motor has certain limitation on the maximum output torque, and the output torque of the motor is constrained as follows through a dynamic model:
Figure FDA0002771048290000091
in the formula (29), θ (t),
Figure FDA0002771048290000092
And
Figure FDA0002771048290000093
respectively motor angle, angular velocity and angular acceleration, JeqAnd BeqRespectively equivalent inertia and equivalent damping of the joint, M is a reduction ratio coefficient, G (theta (t)) is joint connecting rod gravity, and tau (t) and taumaxRespectively the output torque and the rated torque of the motor;
step 2.3.2, adopting time optimal and smoothness optimal multi-objective optimization, and obtaining the expression of the sequence quadratic programming constraint of the optimization model from formulas (26) to (29) as follows:
Figure FDA0002771048290000094
Figure FDA0002771048290000095
in the formulae (30) and (31), hi＝ti+1-ti，kJIs the weight coefficient;
and optimizing the planned track through the sequential quadratic programming constraint of the optimization model.
8. The method for trajectory prediction control of a master-slave surgical robot according to claim 7, wherein the step 3 is specifically:
step 3.1, determining a master-slave mapping function;
and 3.2, determining an operation proportional coefficient, introducing the operation proportional coefficient into the master-slave mapping function in the step 3.1, and mapping the motion poses of the master hand and the slave hand by taking the actual track signal as input to realize the predictive control of the track of the master-slave surgical robot.
9. The master-slave surgical robot trajectory prediction control method according to claim 8, wherein the step 3.1 is specifically:
step 3.1.1, establishing a master hand and slave hand space coordinate system;
step 3.1.2, establishing a master-slave mapping function;
the pose of the main hand operation coordinate system E-XYZ and the display coordinate system D-XYZ is expressed as follows:
Figure FDA0002771048290000101
in the formula (32), the compound represented by the formula (32),
Figure FDA0002771048290000102
a transformation matrix representing the master hand operation coordinate system M-XYZ relative to the display coordinate system D-XYZ,
Figure FDA0002771048290000103
a transformation matrix representing the coordinate system H-XYZ of the end of the master hand relative to the coordinate system M-XYZ of the master hand operation table,
Figure FDA0002771048290000104
a transformation matrix representing the slave operation end coordinate system S-XYZ relative to the slave hand end coordinate system E-XYZ,
Figure FDA0002771048290000105
a transformation matrix representing a slave hand coordinate system T-XYZ relative to a slave operation end coordinate system S-XYZ; therefore, the method comprises the following steps:
Figure FDA0002771048290000106
and (3) carrying out a Cartesian space increment mapping strategy on the formula (33) to obtain a master-slave mapping function:
Figure FDA0002771048290000107
in the formula (34), Ψ represents a control period,
Figure FDA0002771048290000108
is a matrix of positions from the hand(s),
Figure FDA0002771048290000109
is a matrix of the poses of the slave hand,
Figure FDA00027710482900001010
is a matrix of the position of the master hand,
Figure FDA00027710482900001011
is the pose matrix of the master hand.
10. The method for trajectory prediction control of a master-slave surgical robot according to claim 9, wherein the step 3.2 is specifically:
setting λ as a running scaling factor in the master-slave mapping function, and λ only acts on position control, defining λ as:
Figure FDA00027710482900001012
in equation (35), σ is a position magnification factor that determines the size of the hand-made space, and can be obtained by:
Figure FDA0002771048290000111
in the formula (36), DMAnd HMDiameter and height of the working space of the master hand, respectively, DSAnd HSσ is determined from the hand-made space diameter and height, respectively, and from the hand-tip vibration amplitude:
Figure FDA0002771048290000112
in the formula (37), C is the amplitude of vibration from the end of the hand, CΔIs the minimum vibration amplitude of the slave hand in the step signal response;
by introducing operation ratio control in the formula (34), a master-slave pose mapping function can be obtained as follows:
Figure FDA0002771048290000113
and controlling the optimized track through a master-slave pose mapping function, and realizing the prediction control of the track of the master-slave surgical robot.
CN202011249222.9A 2020-11-10 2020-11-10 Master-slave mode surgical robot track prediction control method Pending CN112417755A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011249222.9A CN112417755A (en) 2020-11-10 2020-11-10 Master-slave mode surgical robot track prediction control method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011249222.9A CN112417755A (en) 2020-11-10 2020-11-10 Master-slave mode surgical robot track prediction control method

Publications (1)

Publication Number Publication Date
CN112417755A true CN112417755A (en) 2021-02-26

Family

ID=74781747

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011249222.9A Pending CN112417755A (en) 2020-11-10 2020-11-10 Master-slave mode surgical robot track prediction control method

Country Status (1)

Country Link
CN (1) CN112417755A (en)

Cited By (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113349939A (en) * 2021-07-12 2021-09-07 哈尔滨思哲睿智能医疗设备有限公司 Performance test method and system for passive active hand type master-slave control surgical robot
CN113536983A (en) * 2021-06-29 2021-10-22 辽宁工业大学 Petroleum pipeline stealing positioning method based on P-RLS adaptive filtering time delay estimation
CN113940752A (en) * 2021-11-11 2022-01-18 首都医科大学 Multi-angle projection-based optimal path planning method for pedicle screws
CN114191099A (en) * 2022-01-14 2022-03-18 山东威高手术机器人有限公司 Master-slave tracking delay test method for minimally invasive surgical robot
CN114209435A (en) * 2022-01-14 2022-03-22 山东威高手术机器人有限公司 Indirect surgical robot remote control method comprising prediction and filtering
CN114848155A (en) * 2022-04-29 2022-08-05 电子科技大学 Verification device for delay measurement of surgical robot
CN115424701A (en) * 2022-11-07 2022-12-02 杭州柳叶刀机器人有限公司 Bone surface follow-up technology about optimal path planning
CN115781699A (en) * 2023-02-07 2023-03-14 广东省科学院智能制造研究所 Method, system, equipment and medium for robot motion planning with telecentric constraint
CN115847371A (en) * 2022-07-29 2023-03-28 以诺康医疗科技（苏州）有限公司 Control method and device for master-slave robot
CN118267112A (en) * 2024-04-01 2024-07-02 智九安人工智能科技重庆有限公司 Surgical robot motion control method and system
CN118664567A (en) * 2024-08-22 2024-09-20 北京衔微医疗科技有限公司 Master-slave type ophthalmic surgery robot and instrument axis mapping control method thereof
CN118664567B (en) * 2024-08-22 2024-10-25 北京衔微医疗科技有限公司 Master-slave type ophthalmic surgery robot and instrument axis mapping control method thereof

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
代剑东: "主从式手术机器人震颤滤除与振动抑制方法研究", 《中国优秀硕士学位论文全文数据库》 *
邹水中: "微创手术机器人从手系统控制的研究", 《中国博士学位论文全文数据库》 *

Cited By (20)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113536983A (en) * 2021-06-29 2021-10-22 辽宁工业大学 Petroleum pipeline stealing positioning method based on P-RLS adaptive filtering time delay estimation
CN113536983B (en) * 2021-06-29 2023-12-12 辽宁工业大学 Petroleum pipeline stealing positioning method based on P-RLS adaptive filtering time delay estimation
CN113349939A (en) * 2021-07-12 2021-09-07 哈尔滨思哲睿智能医疗设备有限公司 Performance test method and system for passive active hand type master-slave control surgical robot
CN113940752A (en) * 2021-11-11 2022-01-18 首都医科大学 Multi-angle projection-based optimal path planning method for pedicle screws
CN113940752B (en) * 2021-11-11 2023-07-25 首都医科大学 Multi-angle projection-based pedicle screw optimal path planning method
WO2023134261A1 (en) * 2022-01-14 2023-07-20 山东威高手术机器人有限公司 Master-slave tracking delay test method for minimally invasive surgical robot
CN114191099A (en) * 2022-01-14 2022-03-18 山东威高手术机器人有限公司 Master-slave tracking delay test method for minimally invasive surgical robot
CN114209435B (en) * 2022-01-14 2024-03-22 山东威高手术机器人有限公司 Indirect surgical robot remote control method comprising prediction and filtering
CN114191099B (en) * 2022-01-14 2023-12-01 山东威高手术机器人有限公司 Master-slave tracking delay test method for minimally invasive surgery robot
CN114209435A (en) * 2022-01-14 2022-03-22 山东威高手术机器人有限公司 Indirect surgical robot remote control method comprising prediction and filtering
CN114848155A (en) * 2022-04-29 2022-08-05 电子科技大学 Verification device for delay measurement of surgical robot
CN114848155B (en) * 2022-04-29 2023-04-25 电子科技大学 Verification device for time delay measurement of surgical robot
CN115847371A (en) * 2022-07-29 2023-03-28 以诺康医疗科技（苏州）有限公司 Control method and device for master-slave robot
CN115847371B (en) * 2022-07-29 2023-12-12 以诺康医疗科技（苏州）有限公司 Control method and device for master-slave hand robot
CN115424701B (en) * 2022-11-07 2023-07-21 杭州柳叶刀机器人有限公司 Bone surface follow-up technology for optimal path planning
CN115424701A (en) * 2022-11-07 2022-12-02 杭州柳叶刀机器人有限公司 Bone surface follow-up technology about optimal path planning
CN115781699A (en) * 2023-02-07 2023-03-14 广东省科学院智能制造研究所 Method, system, equipment and medium for robot motion planning with telecentric constraint
CN118267112A (en) * 2024-04-01 2024-07-02 智九安人工智能科技重庆有限公司 Surgical robot motion control method and system
CN118664567A (en) * 2024-08-22 2024-09-20 北京衔微医疗科技有限公司 Master-slave type ophthalmic surgery robot and instrument axis mapping control method thereof
CN118664567B (en) * 2024-08-22 2024-10-25 北京衔微医疗科技有限公司 Master-slave type ophthalmic surgery robot and instrument axis mapping control method thereof

Similar Documents

Publication Publication Date Title
CN112417755A (en) Master-slave mode surgical robot track prediction control method
CN112775976B (en) Task execution control method and device, control equipment and readable storage medium
Long et al. A vibration control method for hybrid-structured flexible manipulator based on sliding mode control and reinforcement learning
Duchaine et al. Computationally efficient predictive robot control
CN105598968B (en) A kind of motion planning and control method of parallel mechanical arm
CN110154024B (en) Assembly control method based on long-term and short-term memory neural network incremental model
CN111230882B (en) Self-adaptive variable impedance control method of fruit sorting parallel robot clamping mechanism
CN112975987B (en) Orthopedic surgery robot control method based on dynamic model
CN109648564B (en) Control method of multi-degree-of-freedom flexible joint mechanical arm system based on hierarchical MPC (Multi-degree-of-freedom)
CN118288294B (en) Robot vision servo and man-machine cooperative control method based on image variable admittance
Tzafestas Adaptive, robust, and fuzzy rule-based control of robotic manipulators
CN117290636A (en) SCARA robot inequality constraint model construction method and model verification method
CN112847373B (en) Robot track synchronous control method and computer readable storage medium
Cruz et al. Application of robust discontinuous control algorithm for a 5-DOF industrial robotic manipulator in real-time
CN113867157B (en) Optimal trajectory planning method and device for control compensation and storage device
Wang et al. Output regulation of the ball and plate system with a nonlinear velocity observer
CN110788859B (en) Controller parameter universe self-adaptive adjustment system
Lange et al. Adaptive minimization of the maximal path deviations of industrial robots
CN114102599A (en) Flexible mechanical arm-based human-computer interaction adaptive control method and system
Hashimoto et al. Visual servoing with linearized observer
Wapenhans et al. Optimal trajectory planning with application to industrial robots
Safeena et al. Adaptive super twisting control of Stewart platform based on super twisting observer
Khajeh et al. Design new intelligent PID like fuzzy backstepping controller
Yang et al. Discrete integral-type zeroing neurodynamics for robust inverse-free and model-free motion control of redundant manipulators
CN117406667B (en) Stretch bender motion control method based on digital twin model

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
RJ01 Rejection of invention patent application after publication
RJ01 Rejection of invention patent application after publication

Application publication date: 20210226