CN113608441A - Position forward-looking self-adaptive puncture control method - Google Patents

Position forward-looking self-adaptive puncture control method Download PDF

Info

Publication number
CN113608441A
CN113608441A CN202110896301.7A CN202110896301A CN113608441A CN 113608441 A CN113608441 A CN 113608441A CN 202110896301 A CN202110896301 A CN 202110896301A CN 113608441 A CN113608441 A CN 113608441A
Authority
CN
China
Prior art keywords
motion
puncture
deceleration
acceleration
puncture outfit
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
CN202110896301.7A
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.)
Nanjing Tuodao Medical Technology Co Ltd
Original Assignee
Nanjing Tuodao Medical Technology Co Ltd
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 Nanjing Tuodao Medical Technology Co Ltd filed Critical Nanjing Tuodao Medical Technology Co Ltd
Priority to CN202110896301.7A priority Critical patent/CN113608441A/en
Publication of CN113608441A publication Critical patent/CN113608441A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
    • G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
    • G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
    • G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B17/00—Surgical instruments, devices or methods, e.g. tourniquets
    • A61B17/34—Trocars; Puncturing needles

Landscapes

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

Abstract

The invention discloses a position look-ahead self-adaptive puncture control method, which comprises the following steps: step 1, obtaining a puncture position according to planning, and accordingly obtaining the distance between the starting position of the puncture outfit and the puncture position; step 2, calculating to obtain acceleration displacement and deceleration displacement according to preset maximum puncture speed, acceleration rate, deceleration rate, maximum acceleration and zero-speed requirement of the puncture outfit when reaching the puncture position, judging whether constant-speed section motion exists or not by combining the step 1, and accordingly obtaining each motion state and performing motion planning; step 3, calculating according to the motion parameters of the current puncture outfit and the motion plan in step 2 to obtain the motion parameters of the puncture outfit at the next moment; wherein the motion parameters comprise the position, the speed and the acceleration of the puncture outfit; and 4, repeating the step 3 until the puncture outfit moves to the puncture position. The invention introduces the foresight adaptive control, solves the problems of execution overshoot and lag of the puncture system and ensures the positioning accuracy and stability of the puncture system.

Description

Position forward-looking self-adaptive puncture control method
Technical Field
The invention relates to the field of machine control, in particular to a position look-ahead self-adaptive puncture control method.
Background
At present, the mainstream puncture process of the spinal surgery mainly depends on the manual completion of an experienced doctor, the doctor determines the puncture position by means of repeated perspective of a C-arm machine in the process, the doctor uses an electric hand drill to drill and puncture in the puncture process, and the doctor needs to complete the puncture under the condition that the electric drill is held to ensure that the puncture direction does not deviate. In order to accurately locate the lesion, a doctor with rich clinical experience is required to complete the process; but in the process: 1. the surgical environment is complicated and severe due to the difference of the spine shape and the position of the focus of different patients; 2. the doctor needs to be exposed in the radiation environment of the C-arm machine for a long time, needs to keep a posture for a long time, and holds the electric drill by hand, so that the situations of body stiffness, fatigue and the like of the doctor can be caused, and the smooth operation of a patient is not facilitated; 3. the radiation is received for a long time, so that the injury to doctors is large, and the working time of the doctors is reduced; 4. the doctor can cause errors of different degrees due to different states of the doctor by manual operation, and the doctor can not reach the focus part completely or exceed the focus part to hurt the patient.
With the progress of computer vision and robotics, the demand for automatic piercing robots has become more and more intense. The existing automatic puncture robot and puncture outfit structure is shown in fig. 1 and 2, a common traditional PID control mode is generally adopted in the prior art, as shown in fig. 3, wherein S is a target position, t1 is a motion stop time, because the traditional PID is based on a feedback control system, certain hysteresis and overshoot conditions exist, a convex part in fig. 3 is a position overshoot, and a patient may be injured due to a slight overshoot in the patient.
Disclosure of Invention
The purpose of the invention is as follows: aiming at the problems, the invention provides a position forward-looking self-adaptive puncture control method, which accurately positions the puncture needle to reach a lesion part and stops, reduces the wound of the puncture needle on a patient in the advancing process, and simultaneously reduces the requirement of the operation on the clinical experience of a doctor.
The technical scheme is as follows:
a position look-ahead adaptive puncture control method comprises the following steps:
step 1, obtaining a puncture position according to planning, and obtaining a distance S between an initial position of a puncture outfit and the puncture position according to the puncture position;
step 2, according to the preset puncture maximum speed V and the acceleration change rate J1The rate of change J of the deceleration2Maximum acceleration amaxAnd the zero speed requirement of the puncture outfit when reaching the puncture position, and calculatingObtaining an acceleration displacement SAddingAnd a deceleration displacement SReducingJudging whether the uniform-speed section motion exists or not by combining the step 1, and accordingly obtaining each motion state and carrying out motion planning;
step 3, calculating according to the motion parameters of the current puncture outfit and the motion plan in step 2 to obtain the motion parameters of the puncture outfit at the next moment; wherein the motion parameters comprise the position, the speed and the acceleration of the puncture outfit;
and 4, repeating the step 3 until the puncture outfit moves to the puncture position.
If S > SAdding+SReducingIf the puncture outfit moves at a constant speed, the stroke of the puncture outfit from the starting point to the puncture position is divided into an acceleration motion state, a constant speed motion state and a deceleration motion state;
if S is less than or equal to SAdding+SReducingIf the puncture outfit does not move at a constant speed, the stroke of the puncture outfit from the starting point to the puncture position is divided into an accelerated motion state and a decelerated motion state;
the acceleration motion state is divided into an acceleration increasing motion stage, a uniform acceleration motion stage and an acceleration reducing motion stage, and the deceleration motion state is divided into a deceleration increasing motion stage, a uniform deceleration motion stage and a deceleration reducing motion stage.
For the case of uniform motion:
defining the position S of the puncture outfit at the starting point0When the time is 0, the robot acquires the position S of the puncture outfit at the time t in real timetVelocity VtAnd acceleration at；
1) If St＜SAddingAnd at the moment, the puncture outfit is in an accelerated motion state, and the motion parameters of the puncture outfit at the next moment are calculated according to the motion parameters of the puncture outfit at the moment t:
2) if St≥SAddingAnd S ist＜S-SReducingAnd at the moment, the puncture outfit is in a uniform motion state, and the motion parameters of the puncture outfit at the next moment are calculated according to the motion parameters of the puncture outfit at the moment t:
3) if St<S, and St≥S-SReducingWhen the puncture outfit is in decelerationAnd (4) calculating the motion parameter of the puncture outfit at the next moment according to the motion parameter of the puncture outfit at the moment t in the motion state.
For the condition of uniform motion, obtaining each motion state and performing motion planning specifically comprises the following steps:
if St＜SAddingAnd when the puncture outfit is in an accelerated motion state, calculating to obtain:
movement time with increased acceleration:
Figure BDA0003198061060000031
time of uniform acceleration motion:
Figure BDA0003198061060000032
movement time with reduced acceleration:
Figure BDA0003198061060000033
if t +1 is less than or equal to tAThen in the acceleration increasing motion phase, at which time JAdding＝J1；
If tA＜t+1≤tBThen in the stage of uniform acceleration motion, at this time JAdding＝0；
If tB＜t+1≤tCIn the acceleration reducing motion phase, at which time JAdding＝-J1；
The motion parameters of the next cycle are thus obtained:
Figure BDA0003198061060000034
Figure BDA0003198061060000035
at+1＝at+JaddingT
If St≥SAddingAnd S ist＜S-SReducingAnd at the moment, the puncture outfit is in a uniform motion stage, and then the motion parameters of the next period are obtained:
St+1＝St+VtT
Vt+1＝V
at+1＝0
and the uniform motion time
Figure BDA0003198061060000036
If St<S, and St≥S-SReducingAnd at the moment, the puncture outfit is in a deceleration motion state, and the following is calculated and obtained:
movement time with increased deceleration:
Figure BDA0003198061060000037
uniform deceleration movement time:
Figure BDA0003198061060000038
movement time with reduced deceleration:
Figure BDA0003198061060000041
if tD＜t+1＜tEThen in the deceleration increasing movement phase, at which time JReducing＝J2；
If tE＜t+1≤tFThen in the stage of uniform deceleration motion, at this time JReducing＝0；
If tF＜t+1≤tGThen in the deceleration decreasing movement phase, at which time JReducing＝-J2；
The motion parameters of the next cycle are thus obtained:
Figure BDA0003198061060000042
Figure BDA0003198061060000043
at+1＝at-JreducingT。
For the case where there is no uniform motion:
defining the position S of the puncture outfit at the starting point0When the time is 0, the robot acquires the position S of the puncture outfit at the time t in real timetVelocity VtAnd acceleration atAnd calculating to obtain the displacement S in the acceleration process according to a variable acceleration displacement formula1And a deceleration process displacement S2，S＝S1+S2；
If St＜S1When the puncture outfit is in an accelerated motion state, calculating the motion parameters of the puncture outfit at the next moment according to the motion parameters of the puncture outfit at the moment t;
if S1≤St<S, calculating the motion parameters of the puncture outfit at the next moment according to the motion parameters of the puncture outfit at the moment t when the puncture outfit is in a deceleration motion state.
For the situation that the uniform motion does not exist, obtaining each motion state and performing the motion planning specifically comprise:
calculating to obtain the displacement S of the acceleration process according to a variable acceleration displacement formula1And a deceleration process displacement S2，S＝S1+S2(ii) a And calculating to obtain the acceleration and deceleration inflection point velocity Vm；
If St＜S1And when the puncture outfit is in an accelerated motion state, calculating to obtain:
movement time with increased acceleration:
Figure BDA0003198061060000044
time of uniform acceleration motion:
Figure BDA0003198061060000045
movement time with reduced acceleration:
Figure BDA0003198061060000051
if t +1 is less than or equal to tAThen in the acceleration increasing motion phase, at which time JAdding＝J1；
If tA＜t+1≤tBThen in the stage of uniform acceleration motion, at this time JAdding＝0；
If tB＜t+1≤tCIn the acceleration reducing motion phase, at which time JAdding＝-J1；
The motion parameters of the next cycle are thus obtained:
Figure BDA0003198061060000052
Figure BDA0003198061060000053
at+1＝at+JaddingT
If S1≤St<S, at this time, the puncture outfit is in a deceleration movement state, and at this time, the deceleration movement state includes a deceleration increasing movement stage, a deceleration uniforming movement stage and a deceleration decreasing movement stage, as shown in fig. 6, and thus, the following are calculated:
movement time with increased deceleration:
Figure BDA0003198061060000054
uniform deceleration movement time:
Figure BDA0003198061060000055
movement time with reduced deceleration:
Figure BDA0003198061060000056
if tC＜t+1＜tDThen in the deceleration increasing movement stage, at this timeJReducing＝J2；
If tD＜t+1≤tEThen in the stage of uniform deceleration motion, at this time JReducing＝0；
If tE＜t+1≤tFThen in the deceleration decreasing movement phase, at which time JReducing＝-J2；
The motion parameters of the next cycle are thus obtained:
Figure BDA0003198061060000057
Figure BDA0003198061060000058
at+1＝at-JreducingT。
In the step 3, the robot acquires the motion parameters of the puncture outfit at the moment T in real time, the stroke of each motion state is discretized into a plurality of points according to each motion state determined in the step 2, and the motion period between the points is T; and performs motion planning accordingly.
Has the advantages that: the invention introduces forward-looking adaptive control, calculates the position, the speed and the acceleration of the puncture outfit in real time, calculates the motion parameters of the puncture outfit at the next moment according to the motion parameters of the current puncture outfit until the puncture outfit moves to the puncture position, thereby ensuring that the motion speed is decelerated to zero speed at the moment of reaching the target position, solving the problem of execution overshoot and lag of the puncture system, ensuring the positioning accuracy and the stability of the puncture system, reducing the wound of the puncture needle to a patient in the advancing process and simultaneously reducing the requirements of the operation on the clinical experience of doctors.
Drawings
FIG. 1 is a diagram of a robotic trolley assembly.
Fig. 2 is a structural view of a conventional automatic puncture system.
Fig. 3 is a waveform diagram of a position in a conventional PID control method.
FIG. 4 is a flow chart of the present invention.
FIG. 5 is a waveform diagram of the acceleration, velocity and position of the prospective adaptive puncturing control method in the presence of uniform motion.
Fig. 6 is a waveform diagram of the motion acceleration, speed and position of the prospective adaptive puncturing control method in the absence of uniform motion.
The automatic puncture device comprises a display 1, a robot system 2, a robot arm 3, an automatic puncture system 4, a body 41, a bone drilling assembly 42, a tracer assembly 43, an indicator light 44 and a puncture device 45.
Detailed Description
The invention is further elucidated with reference to the drawings and the embodiments.
Fig. 1 is a diagram of a conventional robot trolley with automatic puncture, and as shown in fig. 1, the robot trolley includes a display 1, a robot trolley body 2, a robot arm 3, and an automatic puncture system 4, wherein the robot arm 3 and the display 1 are mounted on the robot trolley body 2, and the automatic puncture system 4 is mounted on a terminal joint of the robot arm 3.
Fig. 2 is a structural view of a conventional automatic puncturing system, and as shown in fig. 2, the automatic puncturing system 4 includes a body 41, a bone drilling assembly 42, a tracer assembly 43, an indicator light 44 and a puncturing device 45, wherein the body 41 is fixedly mounted on a terminal joint of the robot arm 3, and a bone drilling motor is disposed therein; the drilling assembly 42 is arranged at the front end of the body 41, and the feeding is controlled by a drilling motor; the puncture outfit 45 is arranged at the front end of the bone drilling assembly 42 and punctures and drills bones along with the feeding of the bone drilling assembly 42; a tracer assembly 43 is mounted on the body 41 for marking the relative position of the automated lancing system; an indicator lamp 44 is mounted on the body 41 for indicating an operating state.
FIG. 4 is a flow chart of the present invention. As shown in fig. 4, the position look-ahead adaptive puncturing control method of the present invention includes the following steps:
step 1, the robot obtains a puncture position according to the plan of a doctor, and obtains a distance S between a puncture outfit and the puncture position according to the puncture position;
step 2, according to the preset puncture maximum speed V and the acceleration change rate J1The rate of change J of the deceleration2And maximum acceleration amaxCalculating the acceleration displacement S for accelerating to the maximum puncture speed VAddingAnd a deceleration displacement S for decelerating from the maximum puncture speed V to 0Reducing：
Figure BDA0003198061060000071
Figure BDA0003198061060000072
Wherein the content of the first and second substances,
Figure BDA0003198061060000073
Figure BDA0003198061060000074
step 3, comparing S and SAdding+SReducingJudging whether the puncture needs to move in a uniform speed section or not, and obtaining each motion state according to the judgment and carrying out motion planning;
step 31, if S > SAdding+SReducingIf so, uniform motion exists, and then the stroke of the puncture outfit moving from the starting point to the puncture position is divided into an acceleration motion state, a uniform motion state and a deceleration motion state, the acceleration motion state is divided into an acceleration increasing motion stage, a uniform acceleration motion stage and an acceleration decreasing motion stage, and the deceleration motion state is divided into a deceleration increasing motion stage, a uniform deceleration motion stage and a deceleration decreasing motion stage; and discretizing the stroke of each motion state into a plurality of points, wherein the motion period between the points is T, and defining the position S of the initial point puncture outfit0Speed V of puncture outfit equal to 000 and the acceleration a of the puncture instrument0＝0；
The robot calculates the position S of the puncture outfit at the moment t in real timetVelocity VtAnd acceleration atAnd caching;
1) if St＜SAddingWhen the puncture outfit is inThe acceleration motion state, in which the acceleration motion state includes an acceleration increasing motion phase, a uniform acceleration motion phase, and an acceleration decreasing motion phase, as shown in fig. 5, is calculated as follows:
movement time with increased acceleration:
Figure BDA0003198061060000081
time of uniform acceleration motion:
Figure BDA0003198061060000082
movement time with reduced acceleration:
Figure BDA0003198061060000083
if t +1 is less than or equal to tAThen in the acceleration increasing motion phase, at which time JAdding＝J1；
If tA＜t+1≤tBThen in the stage of uniform acceleration motion, at this time JAdding＝0；
If tB＜t+1≤tCIn the acceleration reducing motion phase, at which time JAdding＝-J1；
The motion parameters of the next cycle are thus obtained:
Figure BDA0003198061060000084
Figure BDA0003198061060000085
at+1＝at+JaddingT
2) If St≥SAddingAnd S ist＜S-SReducingAnd at the moment, the puncture outfit is in a uniform motion stage, and then the motion parameters of the next period are obtained:
St+1＝St+VtT
Vt+1＝V
at+1＝0
and the uniform motion time
Figure BDA0003198061060000086
3) If St<S, and St≥S-SReducingAt this time, the puncture outfit is in a deceleration movement state, and the deceleration movement state includes a deceleration increasing movement stage, a deceleration homogenizing movement stage and a deceleration decreasing movement stage, as shown in fig. 5, so that the following calculation results:
movement time with increased deceleration:
Figure BDA0003198061060000087
uniform deceleration movement time:
Figure BDA0003198061060000091
movement time with reduced deceleration:
Figure BDA0003198061060000092
if, and tD＜t+1＜tEThen in the deceleration increasing movement phase, at which time JReducing＝J2；
If, and tE＜t+1≤tFThen in the stage of uniform deceleration motion, at this time JReducing＝0；
If tF＜t+1≤tGThen in the deceleration decreasing movement phase, at which time JReducing＝-J2；
The motion parameters of the next cycle are thus obtained:
Figure BDA0003198061060000093
Figure BDA0003198061060000094
at+1＝at-JreducingT
Step 32, if S is less than or equal to SAdding+SReducingIf the puncture outfit does not move at a constant speed, the stroke of the puncture outfit from the starting point to the puncture position is divided into an acceleration motion state and a deceleration motion state, the stroke of each motion state is discretized into a plurality of points, the motion period between each point is T, and the position S of the puncture outfit at the starting point is defined0Speed V of puncture outfit equal to 000 and the acceleration a of the puncture instrument0＝0；
1) Calculating the speed V of the knee point of acceleration and decelerationm：
Setting acceleration process displacement S1Displacement during deceleration S2And then:
Figure BDA0003198061060000095
Figure BDA0003198061060000096
S＝S1+S2
wherein, t1For accelerating the speed of the puncture device to VmTime of (t)2For the speed of the puncture outfit is VmTime to slow down to 0;
Figure BDA0003198061060000097
Figure BDA0003198061060000101
further calculating to obtain the speed V of the acceleration and deceleration inflection pointm：
Figure BDA0003198061060000102
2) The robot calculates the position S of the puncture outfit at the moment t in real timetVelocity VtAnd acceleration atAnd caching;
if St＜S1At this time, the puncture outfit is in an acceleration motion state, the acceleration motion state includes an acceleration increasing motion stage, a uniform acceleration motion stage and an acceleration decreasing motion stage, as shown in fig. 6, and the following calculation results are obtained:
movement time with increased acceleration:
Figure BDA0003198061060000103
time of uniform acceleration motion:
Figure BDA0003198061060000104
movement time with reduced acceleration:
Figure BDA0003198061060000105
if t +1 is less than or equal to tAThen in the acceleration increasing motion phase, at which time JAdding＝J1；
If tA＜t+1≤tBThen in the stage of uniform acceleration motion, at this time JAdding＝0；
If tB＜t+1≤tCIn the acceleration reducing motion phase, at which time JAdding＝-J1；
The motion parameters of the next cycle are thus obtained:
Figure BDA0003198061060000106
Figure BDA0003198061060000107
at+1＝at+JaddingT
If S1≤St<S, at this time, the puncture outfit is in a deceleration movement state, and at this time, the deceleration movement state includes a deceleration increasing movement stage, a deceleration uniforming movement stage and a deceleration decreasing movement stage, as shown in fig. 6, and thus, the following are calculated:
movement time with increased deceleration:
Figure BDA0003198061060000111
uniform deceleration movement time:
Figure BDA0003198061060000112
movement time with reduced deceleration:
Figure BDA0003198061060000113
if tC＜t+1＜tDThen in the deceleration increasing movement phase, at which time JReducing＝J2；
If tD＜t+1≤tEThen in the stage of uniform deceleration motion, at this time JReducing＝0；
If tE＜t+1≤tFThen in the deceleration decreasing movement phase, at which time JReducing＝-J2；
The motion parameters of the next cycle are thus obtained:
Figure BDA0003198061060000114
Figure BDA0003198061060000115
at+1＝at-JreducingT
Step 4, obtaining the motion parameters (position, speed and acceleration) of the puncture outfit at the previous two moments from the cache, writing the motion parameters into a motor driving system, and starting the motion;
step 5, the robot acquires the current position and the speed of the puncture outfit, calculates the motion parameters of the puncture outfit in the next period according to the motion plan in the step 3, and sends the motion parameters to a motor driving system to execute the motion;
and 6, repeating the step 5 until the puncture outfit moves to the puncture position.
In the invention, the stroke of the puncture outfit moving from the starting point to the puncture position is discretized into N points according to the motion state, the motion period among all the points is set as T, and then discretization calculation is carried out, thereby reducing the calculation amount. The invention is not limited to the above, and the position, the speed and the acceleration of the puncture outfit can be calculated by the robot in real time, and the motion parameters of the puncture outfit at the next moment are calculated according to the motion plan until the puncture outfit moves to the puncture position, so that the motion speed is reduced to zero speed when the target position is reached.
Although the preferred embodiments of the present invention have been described in detail, the present invention is not limited to the details of the foregoing embodiments, and various equivalent changes (such as number, shape, position, etc.) may be made to the technical solution of the present invention within the technical spirit of the present invention, and these equivalent changes are all within the protection scope of the present invention.

Claims (7)

1. A position forward-looking self-adaptive puncture control method is characterized by comprising the following steps: the method comprises the following steps:
step 1, obtaining a puncture position according to planning, and obtaining a distance S between an initial position of a puncture outfit and the puncture position according to the puncture position;
step 2, according to the preset puncture maximum speed V and the acceleration change rate J1The rate of change J of the deceleration2Maximum acceleration amaxAnd the zero-speed requirement of the puncture outfit when reaching the puncture position, and the accelerated displacement S is obtained by calculationAddingAnd a deceleration displacement SReducingJudging whether the uniform-speed section motion exists or not by combining the step 1, and accordingly obtaining each motion state and carrying out motion planning;
step 3, calculating according to the motion parameters of the current puncture outfit and the motion plan in step 2 to obtain the motion parameters of the puncture outfit at the next moment; wherein the motion parameters comprise the position, the speed and the acceleration of the puncture outfit;
and 4, repeating the step 3 until the puncture outfit moves to the puncture position.
2. The position look-ahead adaptive puncturing control method according to claim 1, wherein:
if S > SAdding+SReducingIf the puncture outfit moves at a constant speed, the stroke of the puncture outfit from the starting point to the puncture position is divided into an acceleration motion state, a constant speed motion state and a deceleration motion state;
if S is less than or equal to SAdding+SReducingIf the puncture outfit does not move at a constant speed, the stroke of the puncture outfit from the starting point to the puncture position is divided into an accelerated motion state and a decelerated motion state;
the acceleration motion state is divided into an acceleration increasing motion stage, a uniform acceleration motion stage and an acceleration reducing motion stage, and the deceleration motion state is divided into a deceleration increasing motion stage, a uniform deceleration motion stage and a deceleration reducing motion stage.
3. The position look-ahead adaptive puncturing control method according to claim 2, wherein: for the case of uniform motion:
defining the position S of the puncture outfit at the starting point0When the time is 0, the robot acquires the position S of the puncture outfit at the time t in real timetVelocity VtAnd acceleration at；
1) If St＜SAddingWhen the puncture outfit is in an accelerated motion state, calculating the motion parameters of the puncture outfit at the next moment according to the motion parameters of the puncture outfit at the moment t;
2) if St≥SAddingAnd S ist＜S-SReducingWhen the puncture outfit is in a uniform motion state, calculating the motion parameters of the puncture outfit at the next moment according to the motion parameters of the puncture outfit at the moment t;
3) if St< S, and St≥S-SReducingAnd at the moment, the puncture outfit is in a deceleration motion state, and the motion parameter of the puncture outfit at the next moment is calculated according to the motion parameter of the puncture outfit at the moment t.
4. The position look-ahead adaptive puncturing control method according to claim 3, wherein: for the condition of uniform motion, obtaining each motion state and performing motion planning specifically comprises the following steps:
if St＜SAddingAnd when the puncture outfit is in an accelerated motion state, calculating to obtain:
movement time with increased acceleration:
Figure FDA0003198061050000021
time of uniform acceleration motion:
Figure FDA0003198061050000022
movement time with reduced acceleration:
Figure FDA0003198061050000023
if t +1 is less than or equal to tAThen in the acceleration increasing motion phase, at which time JAdding＝J1；
If tA＜t+1≤tBThen in the stage of uniform acceleration motion, at this time JAdding＝0；
If tB＜t+1≤tCIn the acceleration reducing motion phase, at which time JAdding＝-J1；
The motion parameters of the next cycle are thus obtained:
Figure FDA0003198061050000024
Figure FDA0003198061050000025
at+1＝at+JaddingT
If St≥SAddingAnd S ist＜S-SReducingAnd at the moment, the puncture outfit is in a uniform motion stage, and then the motion parameters of the next period are obtained:
St+1＝St+VtT
Vt+1＝V
at+1＝0
and the uniform motion time
Figure FDA0003198061050000026
If St< S, and St≥S-SReducingAnd at the moment, the puncture outfit is in a deceleration motion state, and the following is calculated and obtained:
movement time with increased deceleration:
Figure FDA0003198061050000031
uniform deceleration movement time:
Figure FDA0003198061050000032
movement time with reduced deceleration:
Figure FDA0003198061050000033
if tD＜t+1＜tEThen in the deceleration increasing movement phase, at which time JReducing＝J2；
If tE＜t+1≤tFThen in the stage of uniform deceleration motion, at this time JReducing＝0；
If tF＜t+1≤tGThen in the deceleration decreasing movement phase, at which time JReducing＝-J2；
The motion parameters of the next cycle are thus obtained:
Figure FDA0003198061050000034
Figure FDA0003198061050000035
at+1＝at-JreducingT。
5. The position look-ahead adaptive puncturing control method according to claim 3, wherein: for the case where there is no uniform motion:
defining the position S of the puncture outfit at the starting point0When the time is 0, the robot acquires the position S of the puncture outfit at the time t in real timetVelocity VtAnd acceleration atCalculating the velocity V of the knee point of acceleration and decelerationmAnd calculating to obtain the displacement S in the acceleration process according to a variable acceleration displacement formula1And a deceleration process displacement S2，S＝S1+S2；
If St＜S1When the puncture outfit is in an accelerated motion state, calculating the motion parameters of the puncture outfit at the next moment according to the motion parameters of the puncture outfit at the moment t;
if S1≤StAnd (S), when the puncture outfit is in a deceleration motion state, calculating the motion parameter of the puncture outfit at the next moment according to the motion parameter of the puncture outfit at the moment t.
6. The position look-ahead adaptive puncturing control method according to claim 5, wherein: for the situation that the uniform motion does not exist, obtaining each motion state and performing the motion planning specifically comprise:
if St＜S1And when the puncture outfit is in an accelerated motion state, calculating to obtain:
movement time with increased acceleration:
Figure FDA0003198061050000041
time of uniform acceleration motion:
Figure FDA0003198061050000042
movement time with reduced acceleration:
Figure FDA0003198061050000043
if t +1 is less than or equal to tAThen in the acceleration increasing motion phase, at which time JAdding＝J1；
If tA＜t+1≤tBThen in the stage of uniform acceleration motion, at this time JAdding＝0；
If tB＜t+1≤tCIn the acceleration reducing motion phase, at which time JAdding＝-J1；
The motion parameters of the next cycle are thus obtained:
Figure FDA0003198061050000044
Figure FDA0003198061050000045
at+1＝at+JaddingT
If S1≤St< S, the puncture outfit is in the deceleration motion state, the deceleration motion state comprises a deceleration increasing motion stage, a deceleration homogenizing motion stage and a deceleration decreasing motion stage, as shown in FIG. 6, and the following calculation results are obtained:
movement time with increased deceleration:
Figure FDA0003198061050000046
uniform deceleration movement time:
Figure FDA0003198061050000047
movement time with reduced deceleration:
Figure FDA0003198061050000048
if tC＜t+1＜tDThen in the deceleration increasing movement phase, at which time JReducing＝J2；
If tD＜t+1≤tEThen in the stage of uniform deceleration motion, at this time JReducing＝0；
If tE＜t+1≤tFThen in the deceleration decreasing movement phase, at which time JReducing＝-J2；
The motion parameters of the next cycle are thus obtained:
Figure FDA0003198061050000051
Figure FDA0003198061050000052
at+1＝at-JreducingT。
7. The position look-ahead adaptive puncturing control method according to claim 3, wherein: in the step 3, the robot acquires the motion parameters of the puncture outfit at the moment T in real time, the stroke of each motion state is discretized into a plurality of points according to each motion state determined in the step 2, and the motion period between the points is T; and performs motion planning accordingly.
CN202110896301.7A 2021-08-05 2021-08-05 Position forward-looking self-adaptive puncture control method Pending CN113608441A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110896301.7A CN113608441A (en) 2021-08-05 2021-08-05 Position forward-looking self-adaptive puncture control method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110896301.7A CN113608441A (en) 2021-08-05 2021-08-05 Position forward-looking self-adaptive puncture control method

Publications (1)

Publication Number Publication Date
CN113608441A true CN113608441A (en) 2021-11-05

Family

ID=78307082

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110896301.7A Pending CN113608441A (en) 2021-08-05 2021-08-05 Position forward-looking self-adaptive puncture control method

Country Status (1)

Country Link
CN (1) CN113608441A (en)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS63120313A (en) * 1986-11-10 1988-05-24 Yokogawa Electric Corp Position and speed controller
WO2017113069A1 (en) * 2015-12-28 2017-07-06 深圳配天智能技术研究院有限公司 S-shaped curve planning method and device, and numerically-controlled machine tool
WO2017219639A1 (en) * 2016-06-20 2017-12-28 广州视源电子科技股份有限公司 Motion trail planning method and device for robotic arm, and robot
CN110840534A (en) * 2019-12-19 2020-02-28 上海钛米机器人科技有限公司 Puncture speed planning method and device, puncture equipment and computer storage medium

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS63120313A (en) * 1986-11-10 1988-05-24 Yokogawa Electric Corp Position and speed controller
WO2017113069A1 (en) * 2015-12-28 2017-07-06 深圳配天智能技术研究院有限公司 S-shaped curve planning method and device, and numerically-controlled machine tool
WO2017219639A1 (en) * 2016-06-20 2017-12-28 广州视源电子科技股份有限公司 Motion trail planning method and device for robotic arm, and robot
CN110840534A (en) * 2019-12-19 2020-02-28 上海钛米机器人科技有限公司 Puncture speed planning method and device, puncture equipment and computer storage medium

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
杨超;张冬泉;: "基于S曲线的步进电机加减速的控制", 机电工程, no. 07, pages 813 - 817 *

Similar Documents

Publication Publication Date Title
CN110111880B (en) Artificial potential field path planning method and device based on obstacle grading of flexible needle
CN111315309B (en) System and method for controlling a robotic manipulator or associated tool
CN108748153B (en) Medical robot and control method thereof
Reed et al. Robot-assisted needle steering
CN113855286B (en) Implant robot navigation system and method
CN112336461B (en) Surgical robot, control method, system and readable storage medium
JP2015534845A (en) System and method for underactuated control of insertion path of asymmetric tip needle
CN112603538A (en) Orthopedic navigation positioning system and method
WO2017013828A1 (en) Medical treatment assisting apparatus
CN111650831A (en) Design of interval 2 type fuzzy logic controller of virtual flexible needle in medical robot controller
CN113180828A (en) Operation robot constrained motion control method based on rotation theory
Pisla et al. Kinematic modelling of a 5-DOF hybrid parallel robot for laparoscopic surgery
Jin et al. Design and kinematic analysis of a pedicle screws surgical robot
CN113413214B (en) Surgical robot manpower feedback method and device based on mixed reality guidance
CN113608441A (en) Position forward-looking self-adaptive puncture control method
CN109875659B (en) Flexible needle plane puncture control device and method based on brain emotion learning intelligent control algorithm
CN115192195A (en) Computer-readable storage medium, electronic device, and surgical robot system
CN117323019A (en) Three-operation-arm robot system for urinary puncture operation
CN116392252A (en) Power tool, computer-readable storage medium, and surgical robot system
CN115281802B (en) Control method, equipment and system of puncture device
CN115005979A (en) Computer-readable storage medium, electronic device, and surgical robot system
CN114452004B (en) Control method for tail end position and posture of surgical robot
CN111759464B (en) Control system and method of vascular robot based on bidirectional feedback
CN114569252B (en) Master-slave mapping proportion control system and method for surgical robot
CN114452003B (en) Surgical robot

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
CB02 Change of applicant information

Address after: 210000 building 3, No. 34, Dazhou Road, Yuhuatai District, Nanjing, Jiangsu Province

Applicant after: Tuodao Medical Technology Co.,Ltd.

Address before: Room 102-86, building 6, 57 Andemen street, Yuhuatai District, Nanjing, Jiangsu 210000

Applicant before: Nanjing Tuodao Medical Technology Co.,Ltd.

CB02 Change of applicant information