CN110269683A - A kind of flexible needle tip position based on difference particle filter algorithm and Attitude estimation improved method - Google Patents
A kind of flexible needle tip position based on difference particle filter algorithm and Attitude estimation improved method Download PDFInfo
- Publication number
- CN110269683A CN110269683A CN201910455254.5A CN201910455254A CN110269683A CN 110269683 A CN110269683 A CN 110269683A CN 201910455254 A CN201910455254 A CN 201910455254A CN 110269683 A CN110269683 A CN 110269683A
- Authority
- CN
- China
- Prior art keywords
- state
- particle
- needle
- noise
- matrix
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 239000002245 particle Substances 0.000 title claims abstract description 131
- 238000000034 method Methods 0.000 title claims abstract description 37
- 238000004422 calculation algorithm Methods 0.000 title claims abstract description 30
- 238000005259 measurement Methods 0.000 claims abstract description 26
- 239000011159 matrix material Substances 0.000 claims description 82
- 238000001914 filtration Methods 0.000 claims description 15
- 230000008569 process Effects 0.000 claims description 9
- 238000012952 Resampling Methods 0.000 claims description 8
- 238000005070 sampling Methods 0.000 claims description 7
- 230000002452 interceptive effect Effects 0.000 claims description 6
- 238000004088 simulation Methods 0.000 claims description 6
- 230000003190 augmentative effect Effects 0.000 claims description 5
- 238000012545 processing Methods 0.000 claims description 5
- 238000012935 Averaging Methods 0.000 claims description 3
- 230000015556 catabolic process Effects 0.000 claims description 3
- 238000006731 degradation reaction Methods 0.000 claims description 3
- 230000006872 improvement Effects 0.000 claims description 3
- 238000012886 linear function Methods 0.000 claims description 3
- 238000012546 transfer Methods 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 3
- 238000012795 verification Methods 0.000 claims description 3
- 230000036544 posture Effects 0.000 claims 8
- 230000008030 elimination Effects 0.000 abstract 1
- 238000003379 elimination reaction Methods 0.000 abstract 1
- 238000011160 research Methods 0.000 description 3
- 210000004872 soft tissue Anatomy 0.000 description 3
- 230000003416 augmentation Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000035515 penetration Effects 0.000 description 2
- 108010010803 Gelatin Proteins 0.000 description 1
- 230000009471 action Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 229920000159 gelatin Polymers 0.000 description 1
- 239000008273 gelatin Substances 0.000 description 1
- 235000019322 gelatine Nutrition 0.000 description 1
- 235000011852 gelatine desserts Nutrition 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 238000003384 imaging method Methods 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 238000002324 minimally invasive surgery Methods 0.000 description 1
- 238000011084 recovery Methods 0.000 description 1
- 238000001356 surgical procedure Methods 0.000 description 1
- 210000001519 tissue Anatomy 0.000 description 1
- 238000013334 tissue model Methods 0.000 description 1
- 230000002087 whitening effect Effects 0.000 description 1
Classifications
-
- 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
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C23/00—Combined instruments indicating more than one navigational value, e.g. for aircraft; Combined measuring devices for measuring two or more variables of movement, e.g. distance, speed or acceleration
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Pure & Applied Mathematics (AREA)
- Data Mining & Analysis (AREA)
- Mathematical Optimization (AREA)
- Computational Mathematics (AREA)
- Mathematical Analysis (AREA)
- Theoretical Computer Science (AREA)
- Health & Medical Sciences (AREA)
- Databases & Information Systems (AREA)
- Software Systems (AREA)
- General Engineering & Computer Science (AREA)
- Algebra (AREA)
- Surgery (AREA)
- Life Sciences & Earth Sciences (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Aviation & Aerospace Engineering (AREA)
- Nuclear Medicine, Radiotherapy & Molecular Imaging (AREA)
- Robotics (AREA)
- Operations Research (AREA)
- Computing Systems (AREA)
- Biomedical Technology (AREA)
- Heart & Thoracic Surgery (AREA)
- Medical Informatics (AREA)
- Molecular Biology (AREA)
- Animal Behavior & Ethology (AREA)
- General Health & Medical Sciences (AREA)
- Public Health (AREA)
- Veterinary Medicine (AREA)
- Manipulator (AREA)
- Length Measuring Devices With Unspecified Measuring Means (AREA)
Abstract
The invention discloses a kind of flexible needle tip positions based on difference particle filter algorithm and Attitude estimation improved method, belong to flexible needle control algolithm field.First according to the kinematics characteristic of flexible needle, the pinpoint movement areal model of non-Gaussian noise error is built under inertial coodinate system；Then by pinpoint movement areal model, the coloured noise in conditional extensions elimination model error is first passed through, then the coloured noise in measurement error is eliminated by the similar observation of difference, the extended mode equation containing only white noise is obtained and is simplified.Finally to the extended mode equation after simplification containing only white noise, white noise is handled using difference particle filter, is estimated in conjunction with tip position and posture of the difference particle filter algorithm to flexible needle, simulating, verifying is carried out to the tip position and posture accurately estimated.The model error and measurement error of non-Gaussian noise is effectively treated in the present invention, makes accurate estimation to the position of flexible needle and posture under inertial coodinate system.
Description
Technical Field
The invention belongs to the field of flexible needle control algorithms, and relates to a method for improving the estimation of the position and the posture of a needle point of a flexible needle based on a differential particle filter algorithm.
Background
In recent years, robot-assisted minimally invasive surgery, particularly puncture surgery, has been favored by researchers due to the characteristics of small wound, rapid recovery, and the like. In the puncture operation, due to the fact that human tissue is anisotropic and the individual difference exists, the traditional rigid needle is difficult to achieve accurate control. The patient can suffer from great pain after the needle is withdrawn and reinserted for many times. Thus, researchers have proposed the concept of a flexible needle. The flexible needle has the advantages of small diameter, good elasticity, controllable needle point position and the like, so that the problem of a rigid needle is well solved, and the flexible needle is widely concerned and applied once being put forward.
Research aiming at the flexible needle mainly focuses on the aspects of flexible needle path planning, soft tissue modeling simulation, modeling simulation of interaction of the flexible needle and the soft tissue and the like. However, there has been little research on how to control the flexible needle to precisely reach the action site. This is an essential part of the application of flexible needles and the existing solutions have large errors and need further intensive research.
In the flexible needle control, in order to achieve a high control accuracy, it is necessary to sufficiently grasp information on the position and posture of the needle tip of the flexible needle. However, due to the small cross-sectional diameter of the flexible needle, it is not feasible to place a sensor at the tip of the needle. In gelatin experiments, the position of a needle point is generally measured by adopting a camera imaging technology, and the measurement accuracy of the method is low. Therefore, an estimation algorithm needs to be designed to estimate the position and posture information of the needle tip.
In the estimation, the problems commonly encountered by researchers are: firstly, the noise of the position of the needle point measured by the camera cannot be regarded as Gaussian noise; secondly, because the motion of the flexible needle in the soft tissue is complex, it is difficult to establish an accurate model which can be used for real-time calculation, so in the estimation, the model error of the simplified model cannot be simply regarded as gaussian noise. However, researchers can only process gaussian noise when estimating by using a mature estimation algorithm, and the error is large when estimating the position and the attitude.
Disclosure of Invention
In order to obtain accurate needle point position and attitude estimation information in flexible needle control, the invention improves the traditional estimation method based on differential particle filtering, so that the traditional estimation method based on differential particle filtering can process non-Gaussian noise model errors and measurement errors, and particularly relates to an improved method for estimating the needle point position and attitude of the flexible needle based on a differential particle filtering algorithm.
The method comprises the following specific steps:
step one, according to the kinematic characteristics of a flexible needle, a needle tip kinematic plane model of a non-Gaussian noise error is built under an inertial coordinate system;
the kinematic characteristics of the flexible needle are related to the feed motion of the flexible needle, which effects the penetration of the needle, and to the rotation motion, which varies with the direction of the tip of the needle.
The needle point kinematic plane model comprises model errors and measurement errors, and the specific formula is as follows:
v is the speed at the tip of the flexible needle;the speed of the needle tip in the direction of the x axis under an inertial coordinate system XOY;as the needle tip is in inertial coordinatesA speed in the y-axis direction under XOY;is the included angle between the movement direction of the needle point of the flexible needle and the x axis under the XOY coordinate system; ρ is the radius of curvature of the tip movement.
Secondly, eliminating colored noise in model errors through state expansion of the needle point kinematic plane model, and eliminating colored noise in measurement errors through differential similar observation to obtain an expansion state equation only containing white noise and simplify the expansion state equation;
the method comprises the following specific steps:
firstly, improving a needle tip kinematic plane model to obtain a discrete needle tip kinematic plane model;
the formula is as follows:
ηk+1＝L1ηk+ωk
Δk+1＝L2Δk+vk
status of stateA state vector of 3 × 1;an observation vector of 3 × 1; f (x)k) As a non-linear function shown in a planar model of tip kinematics ηkDriven by white noiseColored noise in model errors, caused by parameter uncertainty or modeling inaccuracies;as colored noise η in model errorkThe input matrix of (2); deltakColored noise in white noise-driven measurement errors, caused by sensor errors; omegakAs colored noise η in model errorkWhite noise random vector of (1), vkFor colored noise Δ in measurement errorkWhite noise random vector of (1); l is1As colored noise η in model errorkThe state gain of (1). L is2For colored noise Δ in measurement errorkThe state gain of (1). k is an iteration parameter, k 1, 2.
Colored noise in model errors is then separately removed η by state expansion and differential homogeneous observationkAnd colored noise Δ in measurement errorkRespectively obtaining an extended state equation only containing white noise and simplifying the extended state equation;
the extended state equation is:
i is an identity matrix;
the simplification results in:
xk+1＝A(xk)xk+Gωk
yk＝Cxk+vk
wherein, an expanded stateA 4 × 1 state vector; observation vectorAugmented state transition matrixAugmented noise transfer matrixThe augmented output matrix C ═ f-L2 B]。
Processing the white noise by using a differential particle filter for the simplified extended state equation only containing the white noise, and estimating the position and the posture of the needle point of the flexible needle by combining a differential particle filtering algorithm;
the specific process of the differential particle filter algorithm is as follows:
step 301, initializing each parameter value and generating particles when the iteration parameter k is equal to 0;
initializing parameters, the parameters including particlesParticle prior informationAnd covariance matrix
The particle prior information formula is:
the covariance matrix formula is:further simplification is as follows:
wherein,is a Cholesky factorization factor; i ═ 1,2, … N, for the ith particle; the state of each particle is a different tip position and pose.
Step 302, when the iteration parameter k is larger than or equal to 1, calculating prior information and a covariance matrix of each particle;
step 303, predicting respective states by using the prior information and covariance matrix of each particle;
the prediction is to carry out one-step prediction on the needle point position and the posture of the flexible needle to be obtained according to the prior information.
Firstly, obtaining a state estimation value and an observation estimation value of the particle by using prior information of the ith particle;
the formula is as follows:
is the estimated value of the state vector of the ith particle;the estimated value of the observation vector of the ith particle is obtained;
then, calculating a covariance matrix by using the state vector estimation value and the observation vector estimation value of the particle;
covariance matrix of state vector estimation values of ith particleThe formula is as follows:
Qka covariance matrix of process noise containing only white noise in the state expansion equation;a first-order mean difference matrix of two values of the state vector of the ith particle and the state vector estimation value; the formula is as follows:d is the difference step length;is a matrixJ is 1,2,3, 4;
the state vector estimation value covariance matrixExpressed as quadratic form:
is a matrixThe observed value of (a);
and step 304, continuously iterating and updating the state values of the particles after the state is predicted, and obtaining the posterior probability to represent the new state of the particles.
First, a first-order mean-difference matrix of the observation vector and the state vector estimated value of the ith particle is calculated
As an observed valueJ is 1,2,3, 4;
then, a first order averaging matrix is usedComputing covariance matrix for one-step prediction in iterative filtering algorithm
Using a first order homodyne matrixAnd observed valueObtaining an interactive covariance matrix
Using covariance matricesAnd interactive covariance matrixObtaining a filter gain matrix
Using filter gain matricesAnd the state estimation value of the particle is combined with the observation vector to obtain the state estimation value of the ith particle
Finally, an estimated covariance matrix of the ith particle is obtained
And representing the covariance matrix of the particle estimation at the previous moment, and calculating through iteration.
305, sampling the particles according to the original particles, the estimated value of the particle state and the covariance matrix obtained by prediction;
the sampling formula is as follows:
step 306, calculating the weight of the particles for the sampled particlesAnd normalized weight
The weight represents the probability that the particle state is close to the true value:
and 307, judging whether residual error resampling exists or not according to the sampled particles and the particle weight, and if so, calculating an estimated value after re-collecting the particle samples. Otherwise, calculating an estimated value;
according to the number of effective samples Neff(Effective sample size) to measure the degree of degradation of the particle set, which is calculated by the formula:
presetting a resampling threshold NthIf N is presenteff＜NthReset the weight toObtaining equal weight sample setThe posterior probability of the filter distributionSystem state estimationAnd estimating the covariance matrixAre respectively as
Where δ (x) is the dirac function.
And 308, returning the residual error resampling result to the predicting and updating step to perform the next iterative computation.
And 309, when the set filtering step length is reached, accurately estimating the position and the posture of the needle point by using a differential particle filtering method.
And fourthly, carrying out simulation verification on the accurately estimated needle tip position and posture.
The invention has the advantages that:
a flexible needle point position and posture estimation improvement method based on a differential particle filter algorithm adjusts a flexible needle kinematic model, and designs a differential particle filter estimation method based on the improved model, so that model errors and measurement errors of non-Gaussian noise can be effectively processed, and the position and posture of a flexible needle can be accurately estimated under an inertial coordinate system.
Drawings
FIG. 1 is a flow chart of the method for improving the position and attitude estimation of the tip of the flexible needle based on the differential particle filter algorithm.
Fig. 2 is a schematic view of a kinematic model of the flexible needle of the present invention.
FIG. 3 is a flow chart of the present invention for processing white noise using a differential particle filter to obtain a tip position and pose estimate;
FIG. 4 is a diagram of simulation results of the present invention.
Detailed Description
The invention will be described in further detail below with reference to the drawings and examples.
The invention provides an improved estimation algorithm based on a differential particle filter algorithm, which is a practical and effective scheme, aiming at the problems that in the estimation of the position and the posture of the needle point of a flexible needle, both model errors and measurement errors are non-Gaussian noises, and a common estimation algorithm is easy to diverge. As shown in fig. 1, the specific steps are as follows:
step one, according to the kinematics characteristic of a flexible needle, constructing a needle tip kinematics plane model of non-Gaussian noise error under an inertial coordinate system XOY;
the kinematic characteristics of the flexible needle are related to the feed motion of the flexible needle, which effects the penetration of the needle, and to the rotation motion, which varies with the direction of the tip of the needle. Like a bicycle, the rear wheel feeds, the front wheel determines the advancing direction, and a flexible needle motion model is built according to the kinematic characteristics of the flexible needle, as shown in fig. 2.
The needle point kinematic plane model comprises model errors and measurement errors, and coordinates where the needle point is located are expressed as (X, Y); the velocity at the tip (X, Y) is then expressed as:
wherein v is the speed at the tip of the flexible needle;for the tip under an inertial frame XOYSpeed in the x-axis direction;the speed of the needle tip in the y-axis direction under an inertial coordinate system XOY;is the included angle between the movement direction of the needle point of the flexible needle and the x axis under the XOY coordinate system;
the kinematic constraints of the needle tip are:
through simultaneous equations (1) and (2), the method can be obtained
Using the illustrated motion relationship, the yaw rate ω can be solved as:
wherein theta is the variation of the included angle between the needle tip and the x axis; ρ is the radius of curvature of the tip movement, which is determined by the tip's angle of rotation r:
the kinematic model of the needle tip obtained in combination with the above formula is as follows:
secondly, improving the needle point kinematic plane model, eliminating colored noise in model errors through state expansion, and eliminating colored noise in measurement errors through differential similar observation to obtain an expansion state equation only containing white noise and simplify the expansion state equation;
the method comprises the following specific steps:
firstly, a needle point kinematic plane model formula (6) is rewritten into a discrete form, and both a model error and a measurement error are set to be non-Gaussian noise, wherein the formula is as follows:
status of stateA state vector of 3 × 1;an observation vector of 3 × 1; f (x)k) Is a non-linear function as shown in equation (6); ηkColored noise in white noise-driven model errors, caused by parameter uncertainty or modeling inaccuracy;as colored noise η in model errorkThe input matrix of (2); deltakColored noise in white noise-driven measurement errors, caused by sensor errors; omegakAs colored noise η in model errorkWhite gaussian noise random vector of (1)kFor colored noise Δ in measurement errorkWhite gaussian noise random vector; l is1As colored noise η in model errorkThe state gain of (1). On the upper part2For colored noise in measurement errorsΔkThe state gain of (1). k is an iteration parameter, k 1, 2.
And then whitening the colored noise, wherein the colored noise cannot be effectively processed in a mature estimation algorithm and can cause divergence of an estimation result, so that the model is expanded into an augmented model to change the colored noise into a part of state quantity, and the method comprises the step of eliminating the colored noise in a model error by state expansion ηkAnd colored noise delta in measurement error can be eliminated through differential homogeneous observation valueskThereby obtaining an extended equation of state of the augmented system containing only white noise.
The extended state equation is:
f is f (x)k) Is a nonlinear function displayed in a flexible needle kinematic plane model; i is an identity matrix; removing the upper marks*And carrying out simplified expression to obtain:
wherein, an expanded stateA 4 × 1 state vector; observation vectorAugmented state transition matrixAugmented noise transfer matrixThe augmented output matrix C ═ f-L2 B]。
Thirdly, establishing a differential particle filter estimator for the simplified extended state equation only containing white noise by combining a differential particle filter theory, processing the white noise by using a differential particle filter, and estimating the position and the posture of the needle point of the flexible needle;
and (4) constructing a differential particle filter estimator according to a simplified augmentation system formula (9). The differential particle filtering method is a mature estimation method capable of processing the Gaussian white noise of both model errors and measurement errors. The invention applies a second-order difference particle filter algorithm, a square root form of a covariance matrix is adopted in a filter equation, partial derivatives of functions do not need to be solved, and the filter algorithm has better numerical characteristics compared with the UKF filter algorithm which can process non-Gaussian noise.
The differential particle filter is divided into two stages, namely an initialization stage and a state and update stage. Firstly, setting an initial value, and then predicting and updating the state by using the model information. As shown in fig. 3, the specific process is as follows:
step 301, initializing each parameter value and generating particles when the iteration parameter k is equal to 0;
initializing parameters, the parameters including particles1,2, … N; particle prior informationAnd covariance matrix
The prior information of the particles is formulated as
The covariance matrix formula is:
further simplification is as follows:
wherein,is a Cholesky factorization factor; n, which represents the ith particle; the state of each particle is a different tip position and pose.
Step 302, predicting and updating the state when the iteration parameter k is more than or equal to 1, and calculating prior information and a covariance matrix of each particle;
step 303, predicting respective states by using the prior information and covariance matrix of each particle;
the prediction is to carry out one-step prediction on the needle point position and the posture of the flexible needle to be obtained according to the prior information.
Firstly, obtaining a state estimation value and an observation estimation value of the particle by using prior information of the ith particle;
the formula is as follows:
a state vector predictor for the ith particle;predicting the observation vector of the ith particle;
then, calculating a covariance matrix by using the state vector estimation value and the observation vector estimation value of the particle;
covariance matrix of state vector predictor of ith particleThe formula is as follows:
Qka covariance matrix of process noise containing only white noise in the state expansion equation;a first-order mean difference matrix of two values of the state vector of the ith particle and the state vector estimation value; the formula is as follows:d is the difference step length;is a matrixJ is 1,2,3, 4;
the state vector estimation value covariance matrixExpressed as quadratic form:
is a matrixThe observed value of (a);
and step 304, continuously iterating and updating the state values of the particles after the state is predicted, and obtaining the posterior probability to represent the new state of the particles.
First, a first-order mean-difference matrix of the observation vector and the state vector estimated value of the ith particle is calculated
As an observed valueJ is 1,2,3, 4;
then, a first order averaging matrix is usedComputing covariance matrix for one-step prediction in iterative filtering algorithm
Using a first order homodyne matrixAnd observed valueObtaining an interactive covariance matrix
Using covariance matricesAnd interactive covariance matrixObtaining a filter gain matrix
Using filter gain matricesAnd the state estimation value of the particle is combined with the observation vector to obtain the state estimation value of the ith particle
Finally, an estimated covariance matrix of the ith particle is obtained
And representing the covariance matrix of the particle estimation at the previous moment, and calculating through iteration.
305, sampling the particles according to the original particles, the estimated value of the particle state and the covariance matrix obtained by prediction;
the sampling formula is as follows:
step 306, calculating the weight of the particles for the sampled particlesAnd normalized weight
The weight represents the probability that the particle state is close to the true value:
and 307, judging whether residual error resampling exists or not according to the sampled particles and the particle weight, and if so, calculating an estimated value after re-collecting the particle samples. Otherwise, calculating an estimated value;
according to the number of effective samples Neff(Effective sample size) to measure the degree of degradation of the particle set, which is calculated by the formula:
presetting a resampling threshold NthIf N is presenteff＜NthReset the weight toObtaining equal weight sample setThe posterior probability of the filter distributionSystem state estimationAnd estimating the covariance matrixAre respectively as
Where δ (x) is the dirac function.
And 308, returning the residual error resampling result to the predicting and updating step to perform the next iterative computation.
And 309, when the set filtering step length is reached, accurately estimating the position and the posture of the needle point by using a differential particle filtering method.
The above procedures are run in an ampere illumination time sequence, and the augmentation state value is accurately predicted by utilizing a differential particle filter algorithmThat is, the position and posture information of the flexible needle can be simultaneously and accurately obtainedAnd non-gaussian noise η.
And fourthly, carrying out simulation verification on the accurately estimated needle tip position and posture.
The state estimation is carried out on the traditional flexible needle model and the improved flexible needle model through programming in matlab, and the state estimation is compared with the real value, and the result is shown in figure 4 and is used for verifying the accuracy of the method provided by the invention.
Claims (4)
1. A flexible needle point position and posture estimation improvement method based on a differential particle filter algorithm is characterized by comprising the following specific steps:
step one, according to the kinematic characteristics of a flexible needle, a needle tip kinematic plane model of a non-Gaussian noise error is built under an inertial coordinate system;
the kinematic characteristics of the flexible needle are related to the feeding motion and the self-rotation motion of the flexible needle, the feeding motion realizes the puncture of the needle, and the self-rotation of the needle is changed along with the direction of the needle point;
the needle point kinematic plane model comprises model errors and measurement errors, and the specific formula is as follows:
v is the speed at the tip of the flexible needle;the speed of the needle tip in the direction of the x axis under an inertial coordinate system XOY;the speed of the needle tip in the y-axis direction under an inertial coordinate system XOY;is the included angle between the movement direction of the needle point of the flexible needle and the x axis under the XOY coordinate system; rho is the curvature radius of the needle tip movement;
secondly, eliminating colored noise in model errors through state expansion of the needle point kinematic plane model, and eliminating colored noise in measurement errors through differential similar observation to obtain an expansion state equation only containing white noise and simplify the expansion state equation;
the method comprises the following specific steps:
firstly, improving a needle tip kinematic plane model to obtain a discrete needle tip kinematic plane model;
the formula is as follows:
ηk+1＝L1ηk+ωk
Δk+1＝L2Δk+vk
status of stateA state vector of 3 × 1;an observation vector of 3 × 1; f (x)k) As a non-linear function shown in a planar model of tip kinematics ηkColored noise in white noise-driven model errors, caused by parameter uncertainty or modeling inaccuracy;as colored noise η in model errorkThe input matrix of (2); deltakColored noise in white noise-driven measurement errors, caused by sensor errors; omegakAs colored noise η in model errorkWhite noise random vector of (1), vkFor colored noise Δ in measurement errorkWhite noise random vector of (1); l is1As colored noise η in model errorkThe state gain of (1). L is2For colored noise Δ in measurement errorkThe state gain of (1). k is an iteration parameter, k ═ 1, 2.. n;
colored noise in model errors is then separately removed η by state expansion and differential homogeneous observationkAnd colored noise Δ in measurement errorkRespectively obtaining an extended state equation only containing white noise and simplifying the extended state equation;
the extended state equation is:
i is an identity matrix;
the simplification results in:
xk+1＝A(xk)xk+Gωk
yk＝Cxk+vk
wherein, an expanded stateA 4 × 1 state vector; observation vectorAugmented state transition matrixAugmented noise transfer matrixThe augmented output matrix C ═ f-L2 B]；
Processing the white noise by using a differential particle filter for the simplified extended state equation only containing the white noise, and estimating the position and the posture of the needle point of the flexible needle by combining a differential particle filtering algorithm;
and fourthly, carrying out simulation verification on the accurately estimated needle tip position and posture.
2. The method for improving the estimation of the position and the attitude of the needle point of the flexible needle based on the differential particle filter algorithm as claimed in claim 1, wherein the differential particle filter algorithm in the third step comprises the following specific processes:
step 301, initializing each parameter value and generating particles when the iteration parameter k is equal to 0;
initializing parameters, the parameters including particlesParticle prior informationAnd covariance matrix
The particle prior information formula is:
the covariance matrix formula is:further simplification is as follows:
wherein,is a Cholesky factorization factor; i ═ 1,2, … N, for the ith particle; the state of each particle is different needle point positions and postures;
step 302, when the iteration parameter k is larger than or equal to 1, calculating prior information and a covariance matrix of each particle;
step 303, predicting respective states by using the prior information and covariance matrix of each particle;
the prediction is to carry out one-step prediction on the needle point position and the posture of the flexible needle to be obtained according to prior information;
step 304, continuously iterating and updating the state values of the particles after the state is predicted, and obtaining a posterior probability to represent as a new state of the particles;
305, sampling the particles according to the original particles, the estimated value of the particle state and the covariance matrix obtained by prediction;
the sampling formula is as follows:
step 306, calculating the weight of the particles for the sampled particlesAnd normalized weight
The weight represents the probability that the particle state is close to the true value:
step 307, judging whether residual error resampling exists or not according to the sampled particles and the particle weight, and if yes, calculating an estimated value after re-collecting the particle samples; otherwise, calculating an estimated value;
according to the number of effective samples Neff(Effective sample size) to measure the degree of degradation of the particle set, which is calculated by the formula:
presetting a resampling threshold NthIf N is presenteff＜NthReset the weight toObtaining equal weight sample setThe posterior probability of the filter distributionSystem state estimationAnd estimating the covariance matrixAre respectively as
Wherein δ (x) is a dirac function;
step 308, returning the residual error re-sampling result to the predicting and updating step for next iterative computation;
and 309, when the set filtering step length is reached, accurately estimating the position and the posture of the needle point by using a differential particle filtering method.
3. The method for improving the estimation of the position and the posture of the needle point of the flexible needle based on the differential particle filter algorithm as claimed in claim 2, wherein the step 303 specifically comprises:
firstly, obtaining a state estimation value and an observation estimation value of the particle by using prior information of the ith particle;
the formula is as follows:
is the estimated value of the state vector of the ith particle;the estimated value of the observation vector of the ith particle is obtained;
then, calculating a covariance matrix by using the state vector estimation value and the observation vector estimation value of the particle;
covariance matrix of state vector estimation values of ith particleThe formula is as follows:
Qka covariance matrix of process noise containing only white noise in the state expansion equation;a first-order mean difference matrix of two values of the state vector of the ith particle and the state vector estimation value; the formula is as follows:d is the difference step length;is a matrixJ is 1,2,3, 4;
the state vector estimation value covariance matrixExpressed as quadratic form: is a matrixThe observed value of (1).
4. The method for improving the estimation of the position and the posture of the needle point of the flexible needle based on the differential particle filter algorithm as claimed in claim 2, wherein the step 304 specifically comprises:
first, a first-order mean-difference matrix of the observation vector and the state vector estimated value of the ith particle is calculated
As an observed valueJ is 1,2,3, 4;
then, a first order averaging matrix is usedComputing covariance matrix for one-step prediction in iterative filtering algorithm
Using a first order homodyne matrixAnd observed valueObtaining an interactive covariance matrix
Using covariance matricesAnd interactive covariance matrixObtaining a filter gain matrix
Using filter gain matricesAnd the state estimation value of the particle is combined with the observation vector to obtain the state estimation value of the ith particle
Finally, an estimated covariance matrix of the ith particle is obtained
And representing the covariance matrix of the particle estimation at the previous moment, and calculating through iteration.
Priority Applications (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910455254.5A CN110269683A (en) | 2019-05-29 | 2019-05-29 | A kind of flexible needle tip position based on difference particle filter algorithm and Attitude estimation improved method |
| CN202010476353.4A CN111603241B (en) | 2019-05-29 | 2020-05-29 | Medical robot positioning device based on differential particle filtering and improvement method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910455254.5A CN110269683A (en) | 2019-05-29 | 2019-05-29 | A kind of flexible needle tip position based on difference particle filter algorithm and Attitude estimation improved method |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN110269683A true CN110269683A (en) | 2019-09-24 |
Family
ID=67959132
Family Applications (2)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910455254.5A Pending CN110269683A (en) | 2019-05-29 | 2019-05-29 | A kind of flexible needle tip position based on difference particle filter algorithm and Attitude estimation improved method |
| CN202010476353.4A Active CN111603241B (en) | 2019-05-29 | 2020-05-29 | Medical robot positioning device based on differential particle filtering and improvement method |
Family Applications After (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010476353.4A Active CN111603241B (en) | 2019-05-29 | 2020-05-29 | Medical robot positioning device based on differential particle filtering and improvement method |
Country Status (1)
| Country | Link |
|---|---|
| CN (2) | CN110269683A (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111796522A (en) * | 2020-07-16 | 2020-10-20 | 上海智驾汽车科技有限公司 | Vehicle state estimation method |
| CN112507597A (en) * | 2020-11-02 | 2021-03-16 | 中国南方电网有限责任公司超高压输电公司广州局 | Multi-cluster particle filter based multi-terminal flexible direct current transmission system state evaluation method |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20070016067A1 (en) * | 2005-05-19 | 2007-01-18 | The Johns Hopkins University | Distal bevel-tip needle control device and algorithm |
| US20110081063A1 (en) * | 2007-09-18 | 2011-04-07 | Koelis | System and method for imaging and locating punctures under prostatic echography |
| CN107655824A (en) * | 2017-11-01 | 2018-02-02 | 哈尔滨理工大学 | A kind of method for estimating flexible pin puncture soft tissue coefficient of friction |
| CN207627384U (en) * | 2017-06-14 | 2018-07-20 | 孙晶 | A kind of high-precision hard film external anesthetic puncture positioning device |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US7049824B2 (en) * | 2003-03-27 | 2006-05-23 | International Business Machines Corporation | Differential particulate detection system for electronic devices |
| US7289906B2 (en) * | 2004-04-05 | 2007-10-30 | Oregon Health & Science University | Navigation system applications of sigma-point Kalman filters for nonlinear estimation and sensor fusion |
| JP6120497B2 (en) * | 2012-06-26 | 2017-04-26 | キヤノン株式会社 | Puncture control device and method |
| CN103256928B (en) * | 2013-04-28 | 2015-05-20 | 南京航空航天大学 | Distributed inertial navigation system and posture transfer alignment method thereof |
| CN106500695B (en) * | 2017-01-05 | 2019-02-01 | 大连理工大学 | A kind of human posture recognition method based on adaptive extended kalman filtering |
| CN108759815B (en) * | 2018-04-28 | 2022-11-15 | 温州大学激光与光电智能制造研究院 | Information fusion integrated navigation method used in global visual positioning method |
-
2019
- 2019-05-29 CN CN201910455254.5A patent/CN110269683A/en active Pending
-
2020
- 2020-05-29 CN CN202010476353.4A patent/CN111603241B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20070016067A1 (en) * | 2005-05-19 | 2007-01-18 | The Johns Hopkins University | Distal bevel-tip needle control device and algorithm |
| US20110081063A1 (en) * | 2007-09-18 | 2011-04-07 | Koelis | System and method for imaging and locating punctures under prostatic echography |
| CN207627384U (en) * | 2017-06-14 | 2018-07-20 | 孙晶 | A kind of high-precision hard film external anesthetic puncture positioning device |
| CN107655824A (en) * | 2017-11-01 | 2018-02-02 | 哈尔滨理工大学 | A kind of method for estimating flexible pin puncture soft tissue coefficient of friction |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111796522A (en) * | 2020-07-16 | 2020-10-20 | 上海智驾汽车科技有限公司 | Vehicle state estimation method |
| CN111796522B (en) * | 2020-07-16 | 2022-06-03 | 上海智驾汽车科技有限公司 | Vehicle state estimation method |
| CN112507597A (en) * | 2020-11-02 | 2021-03-16 | 中国南方电网有限责任公司超高压输电公司广州局 | Multi-cluster particle filter based multi-terminal flexible direct current transmission system state evaluation method |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111603241A (en) | 2020-09-01 |
| CN111603241B (en) | 2023-06-27 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106500695B (en) | A kind of human posture recognition method based on adaptive extended kalman filtering | |
| CN101894278B (en) | Human motion tracing method based on variable structure multi-model | |
| US9019278B2 (en) | Systems and methods for animating non-humanoid characters with human motion data | |
| CN111798491B (en) | Maneuvering target tracking method based on Elman neural network | |
| Dong-Si et al. | Estimator initialization in vision-aided inertial navigation with unknown camera-IMU calibration | |
| CN108115681A (en) | Learning by imitation method, apparatus, robot and the storage medium of robot | |
| JP6776882B2 (en) | Motion analyzers, methods and programs | |
| CN110269683A (en) | A kind of flexible needle tip position based on difference particle filter algorithm and Attitude estimation improved method | |
| CN104567871A (en) | Quaternion Kalman filtering attitude estimation method based on geomagnetic gradient tensor | |
| CN106504275A (en) | A kind of inertial positioning and the real-time three-dimensional method for reconstructing of point cloud registering coupling and complementing | |
| Wang et al. | Robot calligraphy using pseudospectral optimal control in conjunction with a novel dynamic brush model | |
| CN112527119B (en) | Gesture pose data processing method and storage medium | |
| Chiappa et al. | Movement extraction by detecting dynamics switches and repetitions | |
| CN106293103A (en) | Four-axle aircraft gesture control device based on inertial sensor and control method | |
| CN110473291A (en) | Container modeling method and device for container acquisition | |
| CN111189454A (en) | Unmanned vehicle SLAM navigation method based on rank Kalman filtering | |
| CN103839280B (en) | A kind of human body attitude tracking of view-based access control model information | |
| CN103093476A (en) | Three-dimensional object tracking method based on graphics processing unit (GPU) accelerated continuous particle swarm optimization | |
| Bao et al. | 3D perception-based collision-free robotic leaf probing for automated indoor plant phenotyping | |
| Vochten et al. | Robust optimization-based calculation of invariant trajectory representations for point and rigid-body motion | |
| JP2018015436A (en) | Analyzer for behavior of hitting tool | |
| CN113670315B (en) | Variation iterative Kalman filtering-based method for estimating attitude of Liqun heavy tail interference noise dynamic aircraft | |
| CN107869993A (en) | Small satellite attitude method of estimation based on adaptive iteration particle filter | |
| Zhao et al. | Comparison of estimation and control methods for flexible needle in 2D | |
| CN115290076A (en) | Human body joint angle data processing method based on multi-sensor fusion |
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 | ||
| WD01 | Invention patent application deemed withdrawn after publication |
Application publication date: 20190924 |
|
| WD01 | Invention patent application deemed withdrawn after publication |