CN112417755A - A master-slave surgical robot trajectory prediction control method - Google Patents

A master-slave surgical robot trajectory 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
slave
master
trajectory
hand
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—Leader-follower 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)
  • Manipulator (AREA)
  • Feedback Control In General (AREA)

Abstract

本发明公开了一种主从式手术机器人轨迹预测控制方法，首先设计最小二乘支持向量机卡尔曼振动滤波算法，对手部的震颤信号进行滤除，得到目标轨迹信号；分别通过直线轨迹规划法和Chebyshev伪谱法对目标轨迹信号进行轨迹规划，得到规划后的轨迹，通过二次规划法对规划后的轨迹进行优化，得到实际轨迹信号；通过主从位姿映射函数将实际轨迹信号作为输入，映射出主、从手的运动位姿，实现对主从式手术机器人轨迹的预测控制。本发明一种主从式手术机器人轨迹预测控制方法，解决了主从式手术机器人末端运行有延迟、精度低的问题。

Figure 202011249222

The invention discloses a trajectory prediction control method of a master-slave surgical robot. First, a least squares support vector machine Kalman vibration filtering algorithm is designed to filter out the tremor signal of the hand to obtain a target trajectory signal; and Chebyshev pseudospectral method to plan the target trajectory signal to obtain the planned trajectory, optimize the planned trajectory through the quadratic programming method, and obtain the actual trajectory signal; use the master-slave pose mapping function to take the actual trajectory signal as input , map the motion poses of the master and slave hands, and realize the predictive control of the master-slave surgical robot trajectory. The invention provides a trajectory prediction control method for a master-slave surgical robot, which solves the problems of delay and low precision in the terminal operation of the master-slave surgical robot.

Figure 202011249222

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.一种主从式手术机器人轨迹预测控制方法，其特征在于，具体按照以下步骤实施：1. a master-slave type surgical robot trajectory prediction control method, is characterized in that, is specifically implemented according to the following steps: 步骤1，设计最小二乘支持向量机卡尔曼振动滤波算法，对手部的震颤信号进行滤除，得到目标轨迹信号；Step 1, design a least squares support vector machine Kalman vibration filtering algorithm, filter out the tremor signal of the hand, and obtain the target trajectory signal; 步骤2，分别通过直线轨迹规划法和Chebyshev伪谱法对所述目标轨迹信号进行轨迹规划，得到规划后的轨迹，通过二次规划法对规划后的轨迹进行优化，得到实际轨迹信号；Step 2, performing trajectory planning on the target trajectory signal by the linear trajectory planning method and the Chebyshev pseudospectral method, respectively, to obtain the planned trajectory, and optimizing the planned trajectory by the quadratic programming method to obtain the actual trajectory signal; 步骤3，通过主从位姿映射函数将所述实际轨迹信号作为输入，映射出主、从手的运动位姿，实现对主从式手术机器人轨迹的预测控制。In step 3, the actual trajectory signal is used as an input through the master-slave pose mapping function, and the motion poses of the master and slave hands are mapped, so as to realize the predictive control of the master-slave surgical robot trajectory. 2.如权利要求1所述的一种主从式手术机器人轨迹预测控制方法，其特征在于，所述步骤1具体为：2. a kind of master-slave surgical robot trajectory prediction control method as claimed in claim 1 is characterized in that, described step 1 is specifically: 步骤1.1，采集主手的运行轨迹，将其离散化进行标定，将卡尔曼滤波过程写成状态方程与测量过程：Step 1.1, collect the running trajectory of the main hand, discretize it for calibration, and write the Kalman filtering process as a state equation and measurement process: 状态过程：Status process: X(k)＝AX(k-1)+w(k) (1)，X(k)=AX(k-1)+w(k) (1), 测量过程：Measurement process: Z(k)＝HX(k)+v(k) (2)，Z(k)=HX(k)+v(k) (2), 式(1)、(2)中，X(k)是主手中的振动信号的状态，矩阵A表示状态变化，向量Z(k)是信号测量结果，矩阵H表示测量变化，向量w(k)是状态噪声，向量v(k)是测量噪声，其中，w(k)和v(k)是假设相互独立的均值为0的高斯白噪声；In equations (1) and (2), X(k) is the state of the vibration signal in the master's hand, the matrix A represents the state change, the vector Z(k) is the signal measurement result, the matrix H represents the measurement change, and the vector w(k) is the state noise, and the vector v(k) is the measurement noise, where w(k) and v(k) are Gaussian white noise with mean 0 assumed to be independent of each other; 步骤1.2，预测Step 1.2, Predict 根据上一时刻状态的估计值预测下一时刻状态的值，成为先验估计，同时预测下一时刻状态的误差，成为先验误差：Predicting the value of the state at the next moment according to the estimated value of the state at the previous moment becomes a priori estimation, and at the same time predicting the error of the state at the next moment, becomes the prior error:
Figure FDA0002771048290000021
Figure FDA0002771048290000021
 P(k|k-1)＝AP(k-1|k-1)AT+Q (4)，P(k|k-1)=AP(k-1|k-1)A T +Q (4), 式(3)、(4)中，
Figure FDA0002771048290000022
是根据k-1时刻估计k时刻的状态向量，
Figure FDA0002771048290000023
是k-1时刻的后验估计状态向量，P(k|k-1)是根据k-1时刻估计k时刻的协方差，P(k-1|k-1)是k-1时刻的后验估计协方差，Q为过程噪声协方差；In formulas (3) and (4),
Figure FDA0002771048290000022
is the state vector estimated at time k according to time k-1,
Figure FDA0002771048290000023
is the posterior estimated state vector at time k-1, P(k|k-1) is the covariance estimated at time k according to time k-1, and P(k-1|k-1) is the posterior estimation of time k-1 is the experimental estimated covariance, and Q is the process noise covariance; 步骤1.3，更新矫正Step 1.3, update correction 先计算卡尔曼增益，再利用步骤1.2的先验估计计算后验估计，同时更新先验误差到后验误差：Calculate the Kalman gain first, then use the prior estimate in step 1.2 to calculate the posterior estimate, and update the prior error to the posterior error at the same time: K(k)＝P(k|k-1)HT[HP(k|k-1)HT+R]-1 (5)，K(k)=P(k|k-1)H T [HP(k|k-1)H T +R] -1 (5),
Figure FDA0002771048290000024
Figure FDA0002771048290000024
 P(k|k)＝(I-K(k)H)P(k|k-1) (7)，P(k|k)=(I-K(k)H)P(k|k-1) (7), 式(5)、(6)、(7)中，
Figure FDA0002771048290000025
是根据估计状态向量来预测更新k时刻的状态向量，P(k|k)是根据估计状态向量来预测更新k时刻的协方差，I是单位矩阵，K(k)是k时刻的卡尔曼增益，R为测量噪声协方差；In formulas (5), (6) and (7),
Figure FDA0002771048290000025
is to predict and update the state vector at time k according to the estimated state vector, P(k|k) is to predict the covariance at time k according to the estimated state vector, I is the identity matrix, and K(k) is the Kalman gain at time k , R is the measurement noise covariance; 步骤1.4，采用最小二乘支持向量机回归算法来建立R和Q的回归方程，对过程噪声和测量噪声进行处理；Step 1.4, use the least squares support vector machine regression algorithm to establish the regression equations of R and Q, and process the process noise and measurement noise; 步骤1.5，将步骤1.4中的过程噪声协方差Q和测量噪声协方差R的回归方程代入步骤1.2和步骤1.3中，获得改进后的卡尔曼振动滤波算法，对手部的震颤信号进行滤除，得到目标轨迹信号。Step 1.5: Substitute the regression equations of the process noise covariance Q and the measurement noise covariance R in step 1.4 into steps 1.2 and 1.3 to obtain an improved Kalman vibration filtering algorithm, and filter the hand tremor signal to obtain target trajectory signal. 3.如权利要求2所述的一种主从式手术机器人轨迹预测控制方法，其特征在于，所述步骤1.4具体为：3. a kind of master-slave surgical robot trajectory prediction control method as claimed in claim 2 is characterized in that, described step 1.4 is specifically: 步骤1.4.1，建立最小二乘支持向量机训练集为
Figure FDA0002771048290000031
其中Si∈Rn，是测量得到的振动信号和理想控制信号的混合信号，为输入向量，Ri∈R，是混合信号中振动信号的测量噪声协方差；Step 1.4.1, establish the least squares support vector machine training set as
Figure FDA0002771048290000031
where S i ∈ R n , is the mixed signal of the measured vibration signal and the ideal control signal, and is the input vector, and Ri ∈ R is the measured noise covariance of the vibration signal in the mixed signal; 步骤1.4.2，根据最小二乘支持向量机理论，将R和Q的非线性函数的回归问题转化为线性函数的回归问题，将变量R和Q均映射到高维空间F中，得到非线性函数：Step 1.4.2, according to the least squares support vector machine theory, transform the regression problem of nonlinear functions of R and Q into the regression problem of linear functions, and map the variables R and Q into the high-dimensional space F to obtain nonlinear function: R(k)＝ωTφ(s)+b ω∈F，b∈R (8)，R(k)=ω T φ(s)+b ω∈F, b∈R (8), 式(8)中，Φ(s)是非线性映射函数，ω和b是待定参数；In formula (8), Φ(s) is a nonlinear mapping function, and ω and b are undetermined parameters; 步骤1.4.3，根据步骤1.4.2来定义最小二乘支持向量机的优化问题：Step 1.4.3, define the optimization problem of least squares support vector machine according to step 1.4.2:
Figure FDA0002771048290000032
Figure FDA0002771048290000032
 Ri＝ωTφ(Si)+b+ei i＝1，...，N (10)，R i =ω T φ(S i )+b+e i i=1,...,N (10), 式(9)、(10)中，ei是i时刻的松弛变量，C是大于0的常数，式(10)是式(9)的约束条件；In equations (9) and (10), e i is the slack variable at time i, C is a constant greater than 0, and equation (10) is the constraint condition of equation (9); 步骤1.4.4，根据步骤1.4.3列出拉格朗日函数：Step 1.4.4, list the Lagrangian functions according to step 1.4.3:
Figure FDA0002771048290000041
Figure FDA0002771048290000041
 式(11)中，αi∈R，i＝1，...，N，是拉格朗日参数；In formula (11), α i ∈ R, i=1,...,N, are Lagrangian parameters; 步骤1.4.5，对于步骤1.4.3中具有等式约束的一般优化问题，KKT给出了判断求解值是否为最优解的必要条件，对步骤1.4.4所列出的拉格朗日函数的所有参数求偏导数；Step 1.4.5, for the general optimization problem with equality constraints in step 1.4.3, KKT gives the necessary conditions for judging whether the solution value is the optimal solution. For the Lagrangian function listed in step 1.4.4 Find partial derivatives of all parameters of ; 步骤1.4.6，对于步骤1.4.5列出线性方程组：Step 1.4.6, for step 1.4.5 list the system of linear equations:
Figure FDA0002771048290000042
Figure FDA0002771048290000042
 式(12)中，
Figure FDA0002771048290000043
R＝[R1，...，RN]T，α＝[α1，...，αN]T，Ω是核矩阵；In formula (12),
Figure FDA0002771048290000043
R=[R 1 , ..., R N ] T , α=[α 1 , ..., α N ] T , Ω is the kernel matrix; Ωij＝φ(si)TΦ(sj)＝K(si，sj) i，j＝1，...，N (13)，Ω ij =φ(s i ) T Φ(s j )=K(s i , s j ) i,j=1,...,N (13), 式(13)中，K(si，sj)是核函数，Ωij是i行j列的核矩阵；In formula (13), K(s i , s j ) is a kernel function, and Ω ij is a kernel matrix with i row and j column; 步骤1.4.7，对式(12)求解，得到回归方程的系数，即可得到测量噪声协方差R最小二乘支持向量机的回归方程：Step 1.4.7, solve Equation (12), get the coefficients of the regression equation, then get the regression equation of the measurement noise covariance R least squares support vector machine:
Figure FDA0002771048290000044
Figure FDA0002771048290000044
 步骤1.4.8，采用步骤1.4.5与步骤1.4.6建立过程噪声协方差Q的回归方程，具体为：Step 1.4.8, using steps 1.4.5 and 1.4.6 to establish the regression equation of the process noise covariance Q, specifically:
Figure FDA0002771048290000045
Figure FDA0002771048290000045
 式(14)、(15)中，α′i是相对于ai的拉格朗日参数，K(si，sj)′是相对于K(si，sj)的核函数，b′是相对于b的待定参数。In equations (14) and (15), α′ i is the Lagrangian parameter relative to a i , K(s i , s j )′ is the kernel function relative to K(s i , s j ), b ' is an undetermined parameter relative to b. 4.如权利要求2所述的一种主从式手术机器人轨迹预测控制方法，其特征在于，步骤2具体为：4. a kind of master-slave surgical robot trajectory prediction control method as claimed in claim 2 is characterized in that, step 2 is specifically: 步骤2.1，通过直线轨迹规划法对所述目标轨迹信号在任务空间进行轨迹规划；Step 2.1, performing trajectory planning on the target trajectory signal in the task space by using the linear trajectory planning method; 步骤2.2，通过Chebyshev伪谱法对所述目标轨迹信号在关节空间进行轨迹规划；Step 2.2, performing trajectory planning on the target trajectory signal in joint space by Chebyshev pseudospectral method; 步骤2.3，通过二次规划法对步骤2.1和步骤2.2规划后的轨迹进行时间-平滑度优化，得到实际轨迹信号。In step 2.3, the time-smoothness optimization is performed on the trajectory planned in step 2.1 and step 2.2 by a quadratic programming method to obtain an actual trajectory signal. 5.如权利要求4所述的一种主从式手术机器人轨迹预测控制方法，其特征在于，步骤2.1具体为：5. a kind of master-slave surgical robot trajectory prediction control method as claimed in claim 4 is characterized in that, step 2.1 is specifically: 先假定从手机械臂末端点运动在任务空间的起点和终点的位置矢量和姿态矢量分别表示为：It is assumed that the position vector and attitude vector of the start and end points of the movement from the end point of the robot arm in the task space are expressed as: P1＝[x1，y1，z1]T、P2＝[x2，y2，z2]T、
Figure FDA0002771048290000051
，则连接初始点和目标点的位置矢量和姿态矢量分别为：P 1 =[x 1 , y 1 , z 1 ] T , P 2 =[x 2 , y 2 , z 2 ] T ,
Figure FDA0002771048290000051
, then the position vector and attitude vector connecting the initial point and the target point are: P12＝P2-P1＝(x1-x2，y1-y2，z1-z2)T (16)，P 12 =P 2 -P 1 =(x 1 -x 2 , y 1 -y 2 , z 1 -z 2 ) T (16),
Figure FDA0002771048290000052
Figure FDA0002771048290000052
 式(16)、(17)中，[x，y，z]和
Figure FDA0002771048290000053
表示从手机械臂末端点在任务空间坐标系下的位置矢量和姿态矢量；In equations (16) and (17), [x, y, z] and
Figure FDA0002771048290000053
Represents the position vector and attitude vector of the end point of the slave manipulator in the task space coordinate system; 假设从手机械臂末端点从初始值运动到目标值需要时间T，然后将位置矢量和姿态矢量离散化，即可计算出从手机械臂末端点在t时刻的位置矢量和姿态矢量：Assuming that it takes time T to move from the initial value to the target value from the end point of the hand manipulator, and then discretize the position vector and attitude vector, the position vector and attitude vector of the end point of the manipulator arm at time t can be calculated:
Figure FDA0002771048290000061
Figure FDA0002771048290000061
Figure FDA0002771048290000062
Figure FDA0002771048290000062
 式(18)、(19)中，Pt和Rt为t时刻的位置矢量和姿态矢量，In equations (18) and (19), P t and R t are the position vector and attitude vector at time t,
Figure FDA0002771048290000063
Figure FDA0002771048290000064
t表示时刻，t∈(t1，t2)，v和ω分别表示从手机械臂末端点从初始值到目标值的平均线速度和平均角速度。
Figure FDA0002771048290000063
Figure FDA0002771048290000064
t represents time, t∈(t 1 , t 2 ), v and ω represent the average linear velocity and average angular velocity from the initial value to the target value from the end point of the hand manipulator, respectively. 6.如权利要求5所述的一种主从式手术机器人轨迹预测控制方法，其特征在于，所述步骤2.2具体为：6. A kind of master-slave surgical robot trajectory prediction control method as claimed in claim 5, is characterized in that, described step 2.2 is specifically: 步骤2.2.1，变换时间区间Step 2.2.1, change the time interval Chebyshev伪谱法是基于插值多项式近似逼近状态和控制的轨迹，其节点选取的是Chebyshev-Gauss-Lobatto点，定义在区间[-1，1]上，引入新的时间变量τ∈[-1，1]，将原时间区间[t0，tf]转换到[-1，1]，并且t的表达式为：The Chebyshev pseudospectral method is based on the interpolation polynomial approximation to approximate the state and the trajectory of the control. 1], convert the original time interval [t 0 , t f ] to [-1, 1], and the expression of t is:
Figure FDA0002771048290000065
Figure FDA0002771048290000065
 步骤2.2.2，离散节点的计算Step 2.2.2, Calculation of Discrete Nodes Chebyshev伪谱法离散节点选取Chebyshev多项式TN(t)＝cos(Ncos-1(t))的极值点，即CGL点；The Chebyshev pseudospectral method discrete node selects the extreme point of the Chebyshev polynomial T N (t)=cos(Ncos -1 (t)), that is, the CGL point; 步骤2.2.3，状态变量和控制变量的插值近似Step 2.2.3, Interpolation approximation of state variables and control variables 先取N+1个离散点，构造拉格朗日插值多项式作为连续状态和控制变量的近似值，连续状态x(τ)与控制变量u(τ)的近似表达式为：First take N+1 discrete points and construct a Lagrangian interpolation polynomial as the approximate value of the continuous state and the control variable. The approximate expression of the continuous state x(τ) and the control variable u(τ) is:
Figure FDA0002771048290000071
Figure FDA0002771048290000071
Figure FDA0002771048290000072
Figure FDA0002771048290000072
 式(21)、(22)中，xj为j时刻的状态，uj是j时刻的控制变量，其中，φj是拉格朗日插值基函数；In equations (21) and (22), x j is the state at time j, u j is the control variable at time j, where φ j is the Lagrangian interpolation basis function; 步骤2.2.4，动态约束的处理Step 2.2.4, Processing of Dynamic Constraints 对式(21)求导可得连续状态在τk点处的导数
Figure FDA0002771048290000073
近似表达式为：Taking the derivative of equation (21), the derivative of the continuous state at the point τ k can be obtained
Figure FDA0002771048290000073
The approximate expression is:
Figure FDA0002771048290000074
Figure FDA0002771048290000074
 式(23)中，Dkj是(N+1)×(N+1)微分矩阵D第k行第j列元素；In formula (23), D kj is the element of the (N+1)×(N+1) differential matrix D in the kth row and the jth column; 由式(23)得到的
Figure FDA0002771048290000075
代替状态变量对时间的导数，并在插值节点处离散，则原最优控制问题的微分方程约束可转换为代数约束，即对于k＝0，1，...，N，有：Obtained by Eq. (23)
Figure FDA0002771048290000075
Instead of the derivative of the state variable with respect to time and discretized at the interpolation node, the differential equation constraints of the original optimal control problem can be converted into algebraic constraints, that is, for k = 0, 1, ..., N, there are:
Figure FDA0002771048290000076
Figure FDA0002771048290000076
 式(24)中，x(τk)时连续状态，u(τk)是控制变量，τk是CGL点，τf和τ0为新的时间变量；In formula (24), x(τ k ) is a continuous state, u(τ k ) is the control variable, τ k is the CGL point, and τ f and τ 0 are new time variables; 步骤2.2.5，性能指标积分的近似计算Step 2.2.5, Approximate calculation of performance index integral 采用Clenshaw-Curtis数值积分对优化性能指标中的积分项进行近似计算，对于[-1，1]区间上的任一连续函数p(t)，其积分可采用N+1个CGL离散点处的函数值的累加来近似计算，即Clenshaw-Curtis numerical integration is used to approximate the integral term in the optimization performance index. For any continuous function p(t) in the [-1, 1] interval, its integral can be calculated using the integral at N+1 CGL discrete points. The accumulation of function values to approximate the calculation, that is
Figure FDA0002771048290000081
Figure FDA0002771048290000081
 式(25)中，ωk(k＝0，1，...，N)为Clenshaw-Curtis积分权值。In formula (25), ω k (k=0, 1, . . . , N) is the Clenshaw-Curtis integral weight. 7.如权利要求6所述的一种主从式手术机器人轨迹预测控制方法，其特征在于，所述步骤2.3具体为：7. A kind of master-slave surgical robot trajectory prediction control method as claimed in claim 6, is characterized in that, described step 2.3 is specifically: 步骤2.3.1，建立优化模型的约束条件Step 2.3.1, establish the constraints of the optimization model 首先为了操作安全性，从手机械臂的关节一般要采用限位器进行位置限制，为避免超过关节运动范围，应对关节轨迹进行位置约束，具体表述如下：First of all, in order to operate safely, the joints of the slave-hand manipulators generally use limiters to limit the position. In order to avoid exceeding the range of joint motion, the position of the joint trajectories should be constrained. The specific expressions are as follows: θmin≤θ(t)≤θmax (26)，θ min ≤θ(t)≤θ max (26), 式(26)中，θ(t)、θmin和θmax分别为关节角度、关节可转动角度的最小值和最大值；In formula (26), θ(t), θmin and θmax are the minimum and maximum values of the joint angle and the rotatable angle of the joint, respectively; 由于电机和减速器对输入转速具有一定限制，故对关节轨迹的速度约束如下：Since the motor and reducer have certain restrictions on the input speed, the speed constraints on the joint trajectory are as follows:
Figure FDA0002771048290000082
Figure FDA0002771048290000082
Figure FDA0002771048290000083
Figure FDA0002771048290000083
 式(27)、(28)中，
Figure FDA0002771048290000084
和
Figure FDA0002771048290000085
分别为关节角速度和角速度最大值；
Figure FDA0002771048290000086
和
Figure FDA0002771048290000087
分别是关节角加速度和角加速度最大值；In formulas (27) and (28),
Figure FDA0002771048290000084
and
Figure FDA0002771048290000085
are the joint angular velocity and the maximum angular velocity, respectively;
Figure FDA0002771048290000086
and
Figure FDA0002771048290000087
are the joint angular acceleration and the maximum angular acceleration respectively; 手术机器人关节由电机驱动，电机对最大输出力矩有一定限制，通过动力学模型，电机的输出力矩约束为：The surgical robot joint is driven by a motor, and the motor has a certain limit on the maximum output torque. Through the dynamic model, the output torque constraint of the motor is:
Figure FDA0002771048290000091
Figure FDA0002771048290000091
 式(29)中，θ(t)、
Figure FDA0002771048290000092
和
Figure FDA0002771048290000093
分别为电机角度、角速度和角加速度，Jeq和Beq分别为关节的等效惯量和等效阻尼，M为减速比系数，G(θ(t))为关节连杆重力，τ(t)和τmax分别为电机输出转矩和额定转矩；In formula (29), θ(t),
Figure FDA0002771048290000092
and
Figure FDA0002771048290000093
are the motor angle, angular velocity and angular acceleration, respectively, J eq and B eq are the equivalent inertia and equivalent damping of the joint, M is the reduction ratio coefficient, G(θ(t)) is the joint link gravity, τ(t) and τ max are the motor output torque and rated torque, respectively; 步骤2.3.2，采用时间最优和平滑度最优多目标优化，由公式(26)～(29)可得优化模型的序列二次规划约束的表达式为：Step 2.3.2, using the time optimal and smoothness optimal multi-objective optimization, the expression of the sequence quadratic programming constraint of the optimization model can be obtained from formulas (26) to (29):
Figure FDA0002771048290000094
Figure FDA0002771048290000094
Figure FDA0002771048290000095
Figure FDA0002771048290000095
 式(30)、(31)中，hi＝ti+1-ti，kJ是权系数；In formulas (30) and (31), h i =t i+1 -t i , k J is the weight coefficient; 通过优化模型的序列二次规划约束对规划后的轨迹进行优化。The planned trajectory is optimized by the sequential quadratic programming constraints of the optimization model. 8.如权利要求7所述的一种主从式手术机器人轨迹预测控制方法，其特征在于，所述步骤3具体为：8. A kind of master-slave surgical robot trajectory prediction control method as claimed in claim 7, is characterized in that, described step 3 is specifically: 步骤3.1，确定主从映射函数；Step 3.1, determine the master-slave mapping function; 步骤3.2，确定运行比例系数，将运行比例系数引入步骤3.1中的主从映射函数，将实际轨迹信号作为输入，映射出主、从手的运动位姿，实现对主从式手术机器人轨迹的预测控制。Step 3.2, determine the operating scale coefficient, introduce the operating scale coefficient into the master-slave mapping function in step 3.1, use the actual trajectory signal as input, map the motion poses of the master and slave hands, and realize the prediction of the master-slave surgical robot trajectory control. 9.如权利要求8所述的一种主从式手术机器人轨迹预测控制方法，其特征在于，所述步骤3.1具体为：9. A kind of master-slave surgical robot trajectory prediction control method as claimed in claim 8, is characterized in that, described step 3.1 is specifically: 步骤3.1.1，建立主手、从手空间坐标系；Step 3.1.1, establish the master hand and slave hand space coordinate system; 步骤3.1.2，确立主从映射函数；Step 3.1.2, establish the master-slave mapping function; 主手操作坐标系E-XYZ与显示器坐标系D-XYZ位姿表示为：The poses of the main hand operation coordinate system E-XYZ and the display coordinate system D-XYZ are expressed as:
Figure FDA0002771048290000101
Figure FDA0002771048290000101
 式(32)中，
Figure FDA0002771048290000102
表示主手操作坐标系M-XYZ相对于显示器坐标系D-XYZ的变换矩阵，
Figure FDA0002771048290000103
表示主手末端坐标系H-XYZ相对于主手操作台坐标系M-XYZ的变换矩阵，
Figure FDA0002771048290000104
表示从操作端坐标系S-XYZ相对于从手末端坐标系E-XYZ的变换矩阵，
Figure FDA0002771048290000105
表示从手坐标系T-XYZ相对于从操作端坐标系S-XYZ的变换矩阵；故：In formula (32),
Figure FDA0002771048290000102
Represents the transformation matrix of the main hand operation coordinate system M-XYZ relative to the display coordinate system D-XYZ,
Figure FDA0002771048290000103
Represents the transformation matrix of the master-hand end coordinate system H-XYZ relative to the master-hand console coordinate system M-XYZ,
Figure FDA0002771048290000104
Represents the transformation matrix of the slave end coordinate system S-XYZ relative to the slave end coordinate system E-XYZ,
Figure FDA0002771048290000105
Represents the transformation matrix of the slave hand coordinate system T-XYZ relative to the slave operator coordinate system S-XYZ; therefore:
Figure FDA0002771048290000106
Figure FDA0002771048290000106
 将公式(33)进行笛卡尔空间增量映射策略，得到主从映射函数：Applying formula (33) to the Cartesian space incremental mapping strategy, the master-slave mapping function is obtained:
Figure FDA0002771048290000107
Figure FDA0002771048290000107
 式(34)中，Ψ为控制周期，
Figure FDA0002771048290000108
是从手的位置矩阵，
Figure FDA0002771048290000109
是从手的姿态矩阵，
Figure FDA00027710482900001010
是主手的位置矩阵，
Figure FDA00027710482900001011
是主手的姿态矩阵。In formula (34), Ψ is the control period,
Figure FDA0002771048290000108
is the position matrix of the slave hand,
Figure FDA0002771048290000109
is the attitude matrix of the slave hand,
Figure FDA00027710482900001010
is the position matrix of the main hand,
Figure FDA00027710482900001011
is the pose matrix of the main hand. 10.如权利要求9所述的一种主从式手术机器人轨迹预测控制方法，其特征在于，所述步骤3.2具体为：10. A master-slave surgical robot trajectory prediction control method as claimed in claim 9, wherein the step 3.2 is specifically: 在主从映射函数中设置λ为运行缩放系数，并且λ只作用于位置控制，定义λ为：In the master-slave mapping function, set λ as the running scaling factor, and λ only acts on the position control, and define λ as:
Figure FDA00027710482900001012
Figure FDA00027710482900001012
 式(35)中，σ是位置放大系数，其决定了从手工作空间的大小，由此可得：In formula (35), σ is the position magnification factor, which determines the size of the working space of the slave hand, which can be obtained:
Figure FDA0002771048290000111
Figure FDA0002771048290000111
 式(36)中，DM和HM分别是主手工作空间的直径和高，DS和HS分别是从手工作空间的直径和高，从手末端振动幅值决定σ：In formula (36), D M and H M are the diameter and height of the main hand working space, respectively, D S and H S are the diameter and height of the working space of the slave hand, respectively, and σ is determined from the vibration amplitude of the end of the hand:
Figure FDA0002771048290000112
Figure FDA0002771048290000112
 式(37)中，C为从手末端振动幅值的大小，CΔ是从手在阶跃信号响应中的最小振动幅值；In formula (37), C is the magnitude of the vibration amplitude at the end of the slave hand, and C Δ is the minimum vibration amplitude of the slave hand in the step signal response; 在式(34)中引入运行比例控制，可得主从位姿映射函数为：Introducing running proportional control in equation (34), the master-slave pose mapping function can be obtained as:
Figure FDA0002771048290000113
Figure FDA0002771048290000113
 通过主从位姿映射函数对优化后的轨迹进行控制，实现对主从式手术机器人轨迹的预测控制。The optimized trajectory is controlled by the master-slave pose mapping function to realize the predictive control of the master-slave surgical robot trajectory.
CN202011249222.9A 2020-11-10 2020-11-10 A master-slave surgical robot trajectory 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 A master-slave surgical robot trajectory prediction control method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011249222.9A CN112417755A (en) 2020-11-10 2020-11-10 A master-slave surgical robot trajectory 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 A master-slave surgical robot trajectory 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 辽宁工业大学 An oil pipeline theft location method based on P-RLS adaptive filtering 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
CN118662244A (en) * 2024-08-21 2024-09-20 北京衔微医疗科技有限公司 Master-slave type ophthalmic surgery robot and RCM mechanism motion control method thereof
CN118664567A (en) * 2024-08-22 2024-09-20 北京衔微医疗科技有限公司 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 (22)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113536983A (en) * 2021-06-29 2021-10-22 辽宁工业大学 An oil pipeline theft location method based on P-RLS adaptive filtering 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
CN114209435A (en) * 2022-01-14 2022-03-22 山东威高手术机器人有限公司 Indirect surgical robot remote control method comprising prediction and filtering
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
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
CN118662244A (en) * 2024-08-21 2024-09-20 北京衔微医疗科技有限公司 Master-slave type ophthalmic surgery robot and RCM mechanism motion control method thereof
CN118662244B (en) * 2024-08-21 2024-11-01 北京衔微医疗科技有限公司 Master-slave ophthalmic surgical robot and its RCM mechanism motion control method
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 ophthalmic surgical robot and instrument axis mapping control method thereof

Similar Documents

Publication Publication Date Title
CN112417755A (en) A master-slave surgical robot trajectory prediction control method
CN111618858B (en) A Robust Tracking Control Algorithm for Manipulator Based on Adaptive Fuzzy Sliding Mode
Long et al. A vibration control method for hybrid-structured flexible manipulator based on sliding mode control and reinforcement learning
Qi et al. Kinematic control of continuum manipulators using a fuzzy-model-based approach
JP2019509907A (en) Mechanical system control method, mechanical system controller, robot manipulator, and non-transitory computer-readable storage medium
Duchaine et al. Computationally efficient predictive robot control
CN115157238B (en) A dynamic modeling and trajectory tracking method for multi-degree-of-freedom robots
Youcef-Toumi et al. High-speed trajectory control of a direct-drive manipulator
Wu et al. Fuzzy sliding mode variable structure control of a high-speed parallel PnP robot
CN105598968B (en) A kind of motion planning and control method of parallel mechanical arm
CN112975987B (en) Orthopedic surgery robot control method based on dynamic model
Peng et al. Robust adaptive motion/force control scheme for crawler-type mobile manipulator with nonholonomic constraint based on sliding mode control approach
CN109623827A (en) A kind of high-performance redundant degree mechanical arm repetitive motion planning method and device
CN109648564B (en) A control method of multi-degree-of-freedom flexible joint manipulator system based on hierarchical structure MPC
CN112571420B (en) Dual-function model prediction control method under unknown parameters
Rahimi et al. Design and practical implementation of a Neural Network self-tuned Inverse Dynamic Controller for a 3-DoF Delta parallel robot based on Arc Length Function for smooth trajectory tracking
Cruz et al. Application of robust discontinuous control algorithm for a 5-DOF industrial robotic manipulator in real-time
CN112847373B (en) Robot track synchronous control method and computer readable storage medium
Zhang et al. Time delay compensation of a robotic arm based on multiple sensors for indirect teaching
Boscariol et al. Design of a controller for trajectory tracking for compliant mechanisms with effective vibration suppression
Al-Shuka et al. Adaptive hybrid regressor and approximation control of robotic manipulators in constrained space
CN116880170A (en) A model-free adaptive capsule manipulator control method based on active disturbance rejection
Wang et al. Output regulation of the ball and plate system with a nonlinear velocity observer
Korayem et al. New optimization method to solve motion planning of dynamic systems: Application on mechanical manipulators
Liu et al. Research on intelligent control system of manipulator based on multi degree of freedom

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

Application publication date: 20210226

RJ01 Rejection of invention patent application after publication