CN114505859A - Tail end smoothness control method for dental implant surgical robot - Google Patents
Tail end smoothness control method for dental implant surgical robot Download PDFInfo
- Publication number
- CN114505859A CN114505859A CN202210167765.9A CN202210167765A CN114505859A CN 114505859 A CN114505859 A CN 114505859A CN 202210167765 A CN202210167765 A CN 202210167765A CN 114505859 A CN114505859 A CN 114505859A
- Authority
- CN
- China
- Prior art keywords
- freedom
- degree
- dental implant
- mass
- spring
- 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.)
- Granted
Links
- 239000004053 dental implant Substances 0.000 title claims abstract description 96
- 238000000034 method Methods 0.000 title claims abstract description 32
- 238000001356 surgical procedure Methods 0.000 claims abstract description 78
- 238000013016 damping Methods 0.000 claims abstract description 62
- 238000006073 displacement reaction Methods 0.000 claims abstract description 27
- 230000001133 acceleration Effects 0.000 claims abstract description 10
- 230000007613 environmental effect Effects 0.000 claims abstract description 7
- 230000008569 process Effects 0.000 claims abstract description 5
- 238000009825 accumulation Methods 0.000 claims description 18
- 238000013519 translation Methods 0.000 claims description 15
- 239000007943 implant Substances 0.000 claims description 12
- 230000009471 action Effects 0.000 claims description 8
- 230000004044 response Effects 0.000 claims description 5
- 230000008859 change Effects 0.000 abstract description 6
- 238000002513 implantation Methods 0.000 abstract description 3
- 238000010586 diagram Methods 0.000 description 4
- 230000007547 defect Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000013459 approach Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1628—Programme controls characterised by the control loop
- B25J9/1635—Programme controls characterised by the control loop flexible-arm control
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Dental Tools And Instruments Or Auxiliary Dental Instruments (AREA)
Abstract
The invention provides a method for controlling the compliance of the tail end of a dental implant surgical robot, which comprises the following steps: constructing a tail end pose controller of the dental implant surgery robot; constructing a second-order mass-spring-damping system; installing a tail end force-moment sensor and a tail end actual pose detector at the tail end of the dental implant surgery robot, and detecting the force component of the environmental acting force applied to the tail end of the robot in each degree of freedom in real time through the tail end force-moment sensor in the motion process of the dental implant surgery robot; and calculating the acceleration of the mass block in the second-order mass-spring-damping system, converting the acceleration into displacement, inputting the displacement into the end pose controller, and controlling the end of the dental implant surgery robot. The invention provides a method for controlling the compliance of the tail end of a dental implantation surgical robot, which eliminates the shaking phenomenon of the existing industrial robot when a force mode is frequently called, and has the advantages of simple implementation, strong real-time performance, high robustness, smooth control, flexible operation and real-time change of compliance axis.
Description
Technical Field
The invention belongs to the technical field of industrial robot control, and particularly relates to a method for controlling the smoothness of the tail end of a dental implant surgical robot.
Background
At present, with the development of industrial robot technology, the field of dental implants is increasingly adopting industrial cooperative robots to cooperate with oral implanting doctors to implement dental implant operations, and in the implementation process of dental implant operations, the robot is expected to freely push the oral implanting doctors along a planned implanting path in advance, but cannot be pushed or rotated in other directions, namely the implanting path is a flexible axis of the robot. A typical industrial robot has this function, i.e. the common force model of an industrial robot. However, the patient inevitably moves during the operation, which leads to the movement of the planting path, and the flexible shaft of the industrial robot has to be changed. If the existing force mode supported by a common industrial robot is frequently called in an operation, a shaking phenomenon can occur, and the shaking can not only cause the reduction of the planting precision, but also bring unexpected damage to a patient by a more serious person.
Disclosure of Invention
Aiming at the defects in the prior art, the invention provides a method for controlling the end of the implant surgical robot to be compliant, which can effectively solve the problems.
The technical scheme adopted by the invention is as follows:
the invention provides a method for controlling the compliance of the tail end of a dental implant surgical robot, which comprises the following steps:
step S1, constructing a terminal pose controller of the dental implant surgery robot;
step S2, construct a second order mass-spring-damper system F ═ F1,F2,…,F6In which F1,F2,…,F6A second-order mass-spring-damping system respectively representing six degrees of freedom at the tail end of the dental implant surgery robot; six degrees of freedom refer to: the tail end of the dental implant surgery robot is provided with an xyz coordinate system, and the 1 st degree of freedom is the degree of freedom for translation along the x axis; the 2 nd degree of freedom is a translational degree of freedom along the y-axis; the 3 rd degree of freedom is a translational degree of freedom along the z-axis; the 4 th degree of freedom is the degree of freedom of rotation around the x axis; the 5 th degree of freedom is the degree of freedom of rotation around the y axis; the 6 th degree of freedom is the degree of freedom of rotation around the z axis;
for a second order mass-spring-damper system for any ith degree of freedom, i 1, 2. The ith mass block, the ith spring and the ith damper; one side of the bottom of the ith mass block is connected with the support table through an ith spring, and the other side of the bottom of the ith mass block is connected with the support table through an ith damper;
step S3, a terminal force-moment sensor and a terminal actual pose detector are arranged at the tail end of the dental implant surgery robot, and the terminal force-moment sensor is used for detecting the environmental acting force F applied to the tail end of the robot in real time in the movement process of the dental implant surgery roboteThe force component in each degree of freedom is expressed as: fe＝{Fe1，Fe2，...，Fe6In which FeiA force component representing an environmental force experienced by the robot tip in an ith degree of freedom; 1,2, ·, 6;
according to the force component and the moment component of the ith degree of freedom detected by the terminal force-moment sensor in real time, the following two parameters are obtained by calculation:
in the ith degree of freedom, due to a force component FeiVelocity of movement caused by the action of
In the ith degree of freedom, due to a force component FeiIs caused by the action ofei；
Detecting the current terminal actual pose of the dental implant surgical robot in real time through a terminal actual pose detector, wherein the current terminal actual pose comprises a terminal actual position and a terminal actual pose;
step S4, F obtainedei、
And xeiReal-time input to the second-order mass-spring-damping system of the ith degree of freedom, and the acceleration of the mass block in the second-order mass-spring-damping system of the ith degree of freedom is obtained by adopting the following formula
Wherein:
mi: the mass of the second order mass-spring-damper system representing the ith degree of freedom;
bi: a damping coefficient of a damper of a second order mass-spring-damping system representing an ith degree of freedom;
ki: the spring rate of the second order mass-spring-damping system representing the ith degree of freedom;
step S5, the acceleration of the mass block in the second-order mass-spring-damping system of the ith degree of freedom
Integrating to obtain the displacement of the mass block in the second-order mass-spring-damping system with the ith degree of freedom
Step S6, displacement of the mass block in the second-order mass-spring-damping system of the ith degree of freedom
The pose is input to the end pose controller constructed in the step S1 to control the end of the dental implant surgery robot, and the specific method comprises the following steps:
s6.1, the expected end pose comprises an expected end position and an expected end posture; wherein the desired end position comprises a 1 st degree of freedom desired end position, a 2 nd degree of freedom desired end position, and a 3 rd degree of freedom desired end position;
the desired tip poses include a desired tip pose of 4 degrees of freedom, a desired tip pose of 5 degrees of freedom, and a desired tip pose of 6 degrees of freedom;
step S6.2, determining a new desired tip position and a new desired tip pose:
will be 1 st fromDisplacement of a proof mass in a second order Mass-spring-damping System from the desired end position of degrees and 1 st degree of freedom
Performing accumulation operation to obtain a new expected end position targetPosition (1) of the 1 st degree of freedom;
desired end position of 2 degree of freedom and displacement of mass block in 2 degree of freedom second order mass-spring-damping system
Performing accumulation operation to obtain a new expected end position targetPosition (2) of the 2 nd degree of freedom;
desired end position of 3 degree of freedom and displacement of mass in second order mass-spring-damping system of 3 degree of freedom
Performing accumulation operation to obtain a new expected end position targetPosition (3) of the 3 rd degree of freedom;
desired end attitude for 4 th degree of freedom and displacement of mass in second order mass-spring-damping system for 4 th degree of freedom
Performing accumulation operation to obtain a new expected terminal attitude targetPose (4) of the 4 th degree of freedom;
desired end attitude for 5 th degree of freedom and displacement of mass in second order mass-spring-damping system for 5 th degree of freedom
Performing accumulation operation to obtain a new expected terminal attitude targetPose (5) of the 5 th degree of freedom;
desired end attitude for 6 th degree of freedom and displacement of mass in second order mass-spring-damping system for 6 th degree of freedom
Performing accumulation operation to obtain a new expected terminal attitude targetPose (6) of the 6 th degree of freedom;
step S6.3, the method for controlling the end position is:
step S6.3.1, obtaining the terminal position deviation E corresponding to the u-th degree of freedom by adopting the following formulaposition(u)，u＝1，2，3：
Eposition(u)＝targetPosition(u)-currentPosition(u)
Wherein: currentposition (u) represents the actual position of the tail end of the dental implant surgery robot corresponding to the u-th degree of freedom, and is detected by the tail end actual pose detector in the step S3;
step S6.3.2, obtaining the translation linear velocity V of the implant surgical robot end corresponding to the u-th degree of freedom along the robot base coordinate system by adopting the following formulaposition(u)：
Vposition(u)＝kp1(u)*Eposition(u)
Wherein: k is a radical ofp1(u) a proportionality coefficient representing the position deviation of the tail end of the dental implant surgery robot corresponding to the u-th degree of freedom;
step S6.4, the method for controlling the tail end attitude comprises the following steps:
step S6.4.1, obtaining the terminal attitude deviation E corresponding to the v-th degree of freedom by adopting the following formulapose(v)，v＝4，5，6：
Epose(v)＝targetPose(v)-currentPose(v)
Wherein:
currentPose (v) represents the actual pose of the tail end of the dental implant surgical robot corresponding to the v-th degree of freedom, and is detected by the actual pose detector at the tail end in the step S3;
targetPose (v) represents the new expected terminal pose of the dental implant surgical robot corresponding to the v degree of freedom;
step S6.4.2, obtaining the variation speed of the posture deviation of the implant surgery robot end corresponding to the v-th degree of freedom by the following formula
Wherein:
currentErrorPose (v) represents the posture deviation of the current end of the dental implant surgical robot corresponding to the v-th degree of freedom;
preErrorPose (v) represents the last terminal attitude deviation of the dental implant surgery robot corresponding to the v degree of freedom;
step S6.4.3, obtaining the angular velocity V of the implant surgery robot end rotating around the robot base coordinate system corresponding to the V-th degree of freedom by the following formulapose(v)：
Wherein:
kp2(v) a proportionality coefficient representing the posture deviation of the tail end of the dental implant surgery robot corresponding to the v-th degree of freedom;
ka2(v) a proportionality coefficient representing the variation rate of the posture deviation of the tail end of the dental implant surgery robot corresponding to the v-th degree of freedom;
step S6.5, thereby obtaining speed control quantities for six degrees of freedom, respectively: translation linear velocity V corresponding to 1 st degree of freedomposition(1) And the translation linear velocity V corresponding to the 2 nd degree of freedomposition(2) And the translation linear velocity V corresponding to the 3 rd degree of freedomposition(3) Angular velocity V of rotation corresponding to the 4 th degree of freedompose(4) Angular velocity V of rotation corresponding to the 5 th degree of freedompose(5) Angular velocity V of rotation corresponding to the 6 th degree of freedompose(6)；
And generating a speed control instruction by using the speed control quantity with six degrees of freedom, sending the speed control instruction to the tail end mechanical arm of the dental implant surgery robot, and controlling the tail end mechanical arm of the dental implant surgery robot with six degrees of freedom, thereby realizing the flexible control of the tail end of the dental implant surgery robot.
Preferably, the method further comprises the following steps: the compliance of the tail end of the dental implant surgery robot in six degrees of freedom and the response speed are adjusted in real time by adjusting the mass value of a mass block and/or the stiffness coefficient of a spring and/or the damping coefficient of a damper in a second-order mass-spring-damping system corresponding to each degree of freedom.
The invention provides a method for controlling the smoothness of the tail end of a dental implant surgical robot, which has the following advantages:
the invention provides a method for controlling the compliance of the tail end of a dental implantation surgical robot, which eliminates the shaking phenomenon of the existing industrial robot when a force mode is frequently called, and has the advantages of simple implementation, strong real-time performance, high robustness, smooth control, flexible operation and real-time change of compliance axis.
Drawings
Fig. 1 is a structural diagram of an end pose controller of a dental implant surgery robot provided by the invention;
FIG. 2 is a schematic diagram of a mass-damping-spring second-order impedance system employed;
FIG. 3 is a block diagram of a calculation of the mass-damping-spring second order impedance mass displacement;
fig. 4 is a structural diagram of the controller for controlling the compliance of the tail end of the whole dental implant surgery robot.
Detailed Description
In order to make the technical problems, technical solutions and advantageous effects solved by the present invention more clearly apparent, the present invention is further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.
The invention provides a method for controlling the smoothness of the tail end of a dental implant surgical robot, which has the following advantages: firstly, a pose controller for accurately controlling the tail end position of the implant is constructed, and a PID control concept is adopted, so that the pose controller inherits the advantages of simplicity in implementation, high tracking precision, adjustable tracking speed and high tracking stability of the PID controller; in addition, speed ring control is adopted, and position ring control is not adopted, so that the final dental implant surgery robot can approach the expected target position smoothly and without shaking. And secondly, constructing six second-order mass-spring-damping systems, and changing the flexibility and the response speed of the tail end of the dental implant surgery robot in six degrees of freedom by adjusting the value of each mass block, the rigidity value of a spring and the damping coefficient of a damper in the six second-order mass-spring-damping systems. And the adjustment can be changed in real time during the operation according to the change of the planting path. Therefore, the whole method has the advantages of simple implementation, strong real-time performance, high robustness, smooth control, flexible operation and real-time change of the flexible axis.
Aiming at the defects of the prior art in the manual mode of the industrial robot, the invention provides a method for controlling the compliance of the tail end of a dental implantation surgical robot, which can effectively solve the problem of the jitter and comprises the following steps with reference to fig. 4:
step S1, constructing a terminal pose controller of the dental implant surgery robot; as shown in fig. 1, a structural view of an end pose controller;
the input port of the end pose controller is the expected end pose of the dental implant surgery robot, and the output port is a speed control instruction of the mechanical arm at the end of the dental implant surgery robot.
Step S2, construct a second order mass-spring-damper system F ═ F1，F2，...，F6A schematic of a second order mass-spring-damper system is shown in figure 2.
Wherein, F1，F2，...，F6A second-order mass-spring-damping system respectively representing six degrees of freedom at the tail end of the dental implant surgery robot; six degrees of freedom refer to: the tail end of the dental implant surgery robot is provided with an xyz coordinate system, and the 1 st degree of freedom is the degree of freedom for translation along the x axis; the 2 nd degree of freedom is a translational degree of freedom along the y-axis; the 3 rd degree of freedom is a translational degree of freedom along the z-axis; the 4 th degree of freedom is the degree of freedom of rotation around the x axis; the 5 th degree of freedom is the degree of freedom of rotation around the y axis; the 6 th degree of freedom is the degree of freedom of rotation around the z axis;
for a second order mass-spring-damper system for any ith degree of freedom, i 1, 2. The ith mass block, the ith spring and the ith damper; one side of the bottom of the ith mass block is connected with the support table through an ith spring, and the other side of the bottom of the ith mass block is connected with the support table through an ith damper;
in the invention, six second-order mass-spring-damping systems respectively representing six degrees of freedom at the tail end of the dental implant surgery robot are constructed. Each input port of the second-order damping system is a data value of a force-torque sensor at the tail end of the dental implant surgery robot, and the output is the acceleration of mass speed in each second-order mass-spring-damping system.
Step S3, a terminal force-moment sensor and a terminal actual pose detector are arranged at the tail end of the dental implant surgery robot, and the terminal force-moment sensor is used for detecting the environmental acting force F applied to the tail end of the robot in real time in the movement process of the dental implant surgery roboteThe force component in each degree of freedom is expressed as: fe＝{Fe1，Fe2，...，Fe6In which FeiA force component representing an environmental force experienced by the robot tip in an ith degree of freedom; 1,2, ·, 6;
according to the force component and the moment component of the ith degree of freedom detected by the terminal force-moment sensor in real time, the following two parameters are obtained by calculation:
in the ith degree of freedom, due to a force component FeiVelocity of movement caused by the action of
In the ith degree of freedom, due to a force component FeiIs caused by the action ofei；
Detecting the current terminal actual pose of the dental implant surgical robot in real time through a terminal actual pose detector, wherein the current terminal actual pose comprises a terminal actual position and a terminal actual pose;
step S4, F obtainedei、
And xeiSecond order mass-bomb input to ith degree of freedom in real timeThe spring-damping system adopts the following formula to obtain the acceleration of the mass block in the second-order mass-spring-damping system with the ith degree of freedom
Wherein:
mi: the mass of the second order mass-spring-damper system representing the ith degree of freedom;
bi: a damping coefficient of a damper of a second order mass-spring-damping system representing an ith degree of freedom;
ki: the spring rate of the second order mass-spring-damping system representing the ith degree of freedom;
namely, the force and moment of the external environment acting on the tail end of the dental implant surgery robot are detected in real time, the data are obtained through the tail end force-moment sensor of the dental implant surgery robot, and the data act on the six constructed second-order systems to calculate the acceleration of each mass block of the six second-order systems.
Step S5, the acceleration of the mass block in the second-order mass-spring-damping system of the ith degree of freedom
The displacement of the mass block in the second-order mass-spring-damping system with the ith degree of freedom can be obtained by integration, in particular by twice integration
The computational framework is shown in fig. 3.
Step S6, displacement of the mass block in the second-order mass-spring-damping system of the ith degree of freedom
Inputting the pose to the end pose controller constructed in step S1, and performing dental implant surgery on the dental implant surgery robotThe method comprises the following specific steps:
s6.1, the expected end pose comprises an expected end position and an expected end posture; wherein the desired end position comprises a 1 st degree of freedom desired end position, a 2 nd degree of freedom desired end position, and a 3 rd degree of freedom desired end position;
the desired tip poses include a desired tip pose of 4 degrees of freedom, a desired tip pose of 5 degrees of freedom, and a desired tip pose of 6 degrees of freedom;
step S6.2, determining a new desired tip position and a new desired tip pose:
displacement of a proof mass in a second order mass-spring-damping system combining a desired end position of a 1 st degree of freedom and a 1 st degree of freedom
Performing accumulation operation to obtain a new expected end position targetPosition (1) of the 1 st degree of freedom;
desired end position of 2 degree of freedom and displacement of mass block in 2 degree of freedom second order mass-spring-damping system
Performing accumulation operation to obtain a new expected end position targetPosition (2) of the 2 nd degree of freedom;
desired end position of 3 degree of freedom and displacement of mass in second order mass-spring-damping system of 3 degree of freedom
Performing accumulation operation to obtain a new expected end position targetPosition (3) of the 3 rd degree of freedom;
desired end attitude for 4 th degree of freedom and displacement of mass in second order mass-spring-damping system for 4 th degree of freedom
Performing accumulation operation to obtain a new expected terminal attitude of the 4 th degree of freedomtargetPose(4)；
Desired end attitude for 5 th degree of freedom and displacement of mass in second order mass-spring-damping system for 5 th degree of freedom
Performing accumulation operation to obtain a new expected terminal attitude targetPose (5) of the 5 th degree of freedom;
desired end attitude for 6 th degree of freedom and displacement of mass in second order mass-spring-damping system for 6 th degree of freedom
Performing accumulation operation to obtain a new expected terminal attitude targetPose (6) of the 6 th degree of freedom;
the new expected end position and the new expected end pose are input to the robot end pose controller, and the dental implant surgery robot can be controlled to accurately reach the new end expected pose. Specifically, step S6.3 and step S6.4 are included.
Step S6.3, the method for controlling the end position is:
step S6.3.1, obtaining the end position deviation E corresponding to the u-th degree of freedom using the following formulaposition(u)，u＝1，2，3：
Eposition(u)＝targetPosition(u)-currentPosition(u)
Wherein: currentposition (u) represents the actual position of the tail end of the dental implant surgery robot corresponding to the u-th degree of freedom, and is detected by the tail end actual pose detector in the step S3;
step S6.3.2, obtaining the translation linear velocity V of the implant surgical robot end corresponding to the u-th degree of freedom along the robot base coordinate system by adopting the following formulaposition(u)：
Vposition(u)＝kp1(u)*Eposition(u)
Wherein: k is a radical ofp1(u) a proportionality coefficient representing the position deviation of the tail end of the dental implant surgery robot corresponding to the u-th degree of freedom;
step S6.4, the method for controlling the tail end attitude comprises the following steps:
step S6.4.1, obtaining the terminal attitude deviation E corresponding to the v-th degree of freedom by adopting the following formulapose(v)，v＝4，5，6：
Epose(v)＝targetPose(v)-currentPose(v)
Wherein:
currentPose (v) represents the actual pose of the tail end of the dental implant surgical robot corresponding to the v-th degree of freedom, and is detected by the actual pose detector at the tail end in the step S3;
targetPose (v) represents the new expected terminal pose of the dental implant surgical robot corresponding to the v degree of freedom;
step S6.4.2, obtaining the variation speed of the posture deviation of the implant surgery robot end corresponding to the v-th degree of freedom by the following formula
Wherein:
currentErrorPose (v) represents the posture deviation of the current end of the dental implant surgical robot corresponding to the v-th degree of freedom;
preErrorPose (v) represents the last terminal attitude deviation of the dental implant surgery robot corresponding to the v degree of freedom;
step S6.4.3, obtaining the angular velocity V of the implant surgery robot end rotating around the robot base coordinate system corresponding to the V-th degree of freedom by the following formulapose(v)：
Wherein:
kp2(v) a proportionality coefficient representing the posture deviation of the tail end of the dental implant surgery robot corresponding to the v-th degree of freedom;
kd2(v) represents the v-th degree of freedom pairProportional coefficient of variation rate of terminal attitude deviation of dental implant surgery robot;
step S6.5, thereby obtaining speed control quantities for six degrees of freedom, respectively: translation linear velocity V corresponding to 1 st degree of freedomposition(1) And the translation linear velocity V corresponding to the 2 nd degree of freedomposition(2) And the translation linear velocity V corresponding to the 3 rd degree of freedomposition(3) Angular velocity V of rotation corresponding to the 4 th degree of freedompose(4) Angular velocity V of rotation corresponding to the 5 th degree of freedompose(5) Angular velocity V of rotation corresponding to the 6 th degree of freedompose(6)；
And generating a speed control instruction by using the speed control quantity with six degrees of freedom, sending the speed control instruction to the tail end mechanical arm of the dental implant surgery robot, and controlling the tail end mechanical arm of the dental implant surgery robot with six degrees of freedom, thereby realizing the flexible control of the tail end of the dental implant surgery robot.
According to the requirements of an actual operation, the flexibility of the tail end of the dental implant operation robot in six degrees of freedom and the response speed are adjusted in real time by adjusting the mass value of the mass block in the second-order mass-spring-damping system corresponding to each degree of freedom and/or the stiffness coefficient of the spring and/or the damping coefficient of the damper, so that the requirements of the flexibility in different degrees of freedom in the actual operation are met.
It is emphasized that the end pose controller of the dental implant surgery robot constructed by the invention is a speed instruction instead of a position instruction, and the control instruction finally sent to the dental implant surgery robot is the speed instruction.
The invention provides a method for controlling the smoothness of the tail end of a dental implant surgical robot, which comprises the following steps: firstly, a pose controller capable of accurately controlling the expected pose of the tail end of the dental implant surgery robot is constructed, an input port of the pose controller is the expected pose of the tail end of the dental implant surgery robot, an output port of the pose controller is a speed control command of the tail end of the dental implant surgery robot, then six second-order systems constructed by mass-spring-damping are introduced into the pose controller, an input port of each of the six second-order systems is force-torque data read by a force-torque sensor at the tail end of the dental implant surgery robot, an output port of the pose controller is a pose deviation value under the action of real-time force-torque data, and the pose deviation value is connected to an input end of the pose controller in an adder mode. Therefore, the pose controller constructed in the way can dynamically respond to the change of the terminal pose of the implant surgery robot under the action of external force in real time. The values of the mass blocks, the rigidity values of the springs and the damping values of the dampers in the six mass-spring-damping systems are respectively changed to control the response displacement values of the tail end of the dental implant surgery robot to the data of the force-torque sensor in different directions, so that the flexible control of the tail end of the dental implant surgery robot in different directions can be achieved as required. The control method constructed by the invention eliminates the shaking phenomenon of the existing industrial robot when the force mode is frequently called, and has the advantages of simple implementation, high solving precision, strong real-time performance, high robustness, smooth control, flexible operation and real-time change of the compliance axis (real-time variable compliance direction).
According to the method for controlling the end flexibility of the dental implant surgery robot, provided by the invention, the end flexibility controller of the dental implant surgery robot can be prepared.
The foregoing is only a preferred embodiment of the present invention, and it should be noted that, for those skilled in the art, various modifications and improvements can be made without departing from the principle of the present invention, and such modifications and improvements should also be considered within the scope of the present invention.
Claims (2)
1. A method for controlling the smoothness of the tail end of a dental implant surgical robot is characterized by comprising the following steps:
step S1, constructing a terminal pose controller of the dental implant surgery robot;
step S2, construct a second order mass-spring-damper system F ═ F1,F2,…,F6In which F1,F2,…,F6A second-order mass-spring-damping system respectively representing six degrees of freedom at the tail end of the dental implant surgery robot; six degrees of freedom refer to: dental implant surgery machineThe tail end of the robot is provided with an xyz coordinate system, and the 1 st degree of freedom is a translation degree of freedom along the x axis; the 2 nd degree of freedom is a translational degree of freedom along the y-axis; the 3 rd degree of freedom is a translational degree of freedom along the z-axis; the 4 th degree of freedom is the degree of freedom of rotation around the x axis; the 5 th degree of freedom is the degree of freedom of rotation around the y axis; the 6 th degree of freedom is the degree of freedom of rotation around the z axis;
for a second order mass-spring-damper system for any ith degree of freedom, i-1, 2, …,6, comprising: the ith mass block, the ith spring and the ith damper; one side of the bottom of the ith mass block is connected with the support table through an ith spring, and the other side of the bottom of the ith mass block is connected with the support table through an ith damper;
step S3, a terminal force-moment sensor and a terminal actual pose detector are arranged at the tail end of the dental implant surgery robot, and the terminal force-moment sensor is used for detecting the environmental acting force F applied to the tail end of the robot in real time in the movement process of the dental implant surgery roboteThe force component in each degree of freedom is expressed as: fe＝{Fe1,Fe2,…,Fe6In which FeiA force component representing an environmental force experienced by the robot tip in an ith degree of freedom; 1,2, …, 6;
according to the force component and the moment component of the ith degree of freedom detected by the terminal force-moment sensor in real time, the following two parameters are obtained by calculation:
in the ith degree of freedom, due to a force component FeiVelocity of movement caused by the action of
In the ith degree of freedom, due to a force component FeiIs caused by the action ofei；
Detecting the current terminal actual pose of the dental implant surgical robot in real time through a terminal actual pose detector, wherein the current terminal actual pose comprises a terminal actual position and a terminal actual pose;
step S4, F obtainedei、
And xeiReal-time input to the second-order mass-spring-damping system of the ith degree of freedom, and the acceleration of the mass block in the second-order mass-spring-damping system of the ith degree of freedom is obtained by adopting the following formula
Wherein:
mi: the mass of the second order mass-spring-damper system representing the ith degree of freedom;
bi: a damping coefficient of a damper of a second order mass-spring-damping system representing an ith degree of freedom;
ki: the spring rate of the second order mass-spring-damping system representing the ith degree of freedom;
step S5, the acceleration of the mass block in the second-order mass-spring-damping system of the ith degree of freedom
Integrating to obtain the displacement of the mass block in the second-order mass-spring-damping system with the ith degree of freedom
Step S6, displacement of the mass block in the second-order mass-spring-damping system of the ith degree of freedom
The pose is input to the end pose controller constructed in the step S1 to control the end of the dental implant surgery robot, and the specific method comprises the following steps:
s6.1, the expected end pose comprises an expected end position and an expected end posture; wherein the desired end position comprises a 1 st degree of freedom desired end position, a 2 nd degree of freedom desired end position, and a 3 rd degree of freedom desired end position;
the desired tip poses include a desired tip pose of 4 degrees of freedom, a desired tip pose of 5 degrees of freedom, and a desired tip pose of 6 degrees of freedom;
step S6.2, determining a new desired tip position and a new desired tip pose:
displacement of a proof mass in a second order mass-spring-damping system combining a desired end position of a 1 st degree of freedom and a 1 st degree of freedom
Performing accumulation operation to obtain a new expected end position targetPosition (1) of the 1 st degree of freedom;
desired end position of 2 degree of freedom and displacement of mass block in 2 degree of freedom second order mass-spring-damping system
Performing accumulation operation to obtain a new expected end position targetPosition (2) of the 2 nd degree of freedom;
desired end position of 3 degree of freedom and displacement of mass in second order mass-spring-damping system of 3 degree of freedom
Performing accumulation operation to obtain a new expected end position targetPosition (3) of the 3 rd degree of freedom;
desired end attitude for 4 th degree of freedom and displacement of mass in second order mass-spring-damping system for 4 th degree of freedom
Performing accumulation operation to obtain a new expected terminal attitude targetPose (4) of the 4 th degree of freedom;
desired end attitude for 5 th degree of freedom and displacement of mass in second order mass-spring-damping system for 5 th degree of freedom
Performing accumulation operation to obtain a new expected terminal attitude targetPose (5) of the 5 th degree of freedom;
desired end attitude for 6 th degree of freedom and displacement of mass in second order mass-spring-damping system for 6 th degree of freedom
Performing accumulation operation to obtain a new expected terminal attitude targetPose (6) of the 6 th degree of freedom;
step S6.3, the method for controlling the end position is:
step S6.3.1, obtaining the end position deviation E corresponding to the u-th degree of freedom using the following formulaposition(u),u＝1,2,3：
Eposition(u)＝targetPosition(u)-currentPosition(u)
Wherein: currentposition (u) represents the actual position of the tail end of the dental implant surgery robot corresponding to the u-th degree of freedom, and is detected by the tail end actual pose detector in the step S3;
step S6.3.2, obtaining the translation linear velocity V of the implant surgical robot end corresponding to the u-th degree of freedom along the robot base coordinate system by adopting the following formulaposition(u)：
Vposition(u)＝kp1(u)*Eposition(u)
Wherein: k is a radical ofp1(u) a proportionality coefficient representing the position deviation of the tail end of the dental implant surgery robot corresponding to the u-th degree of freedom;
step S6.4, the method for controlling the tail end attitude comprises the following steps:
step S6.4.1, obtaining the terminal attitude deviation E corresponding to the v-th degree of freedom by adopting the following formulapose(v),v＝4,5,6：
Epose(v)＝targetPose(v)-currentPose(v)
Wherein:
currentPose (v) represents the actual pose of the tail end of the dental implant surgical robot corresponding to the v-th degree of freedom, and is detected by the actual pose detector at the tail end in the step S3;
targetpos (v) represents the new expected end pose of the dental implant surgery robot corresponding to the v-th degree of freedom;
step S6.4.2, obtaining the variation speed of the posture deviation of the implant surgery robot end corresponding to the v-th degree of freedom by the following formula
Wherein:
currentErrorPose (v) represents the posture deviation of the current end of the dental implant surgical robot corresponding to the v-th degree of freedom;
preErrorPose (v) represents the last terminal attitude deviation of the dental implant surgery robot corresponding to the v degree of freedom;
step S6.4.3, obtaining the angular velocity V of the implant surgery robot end rotating around the robot base coordinate system corresponding to the V-th degree of freedom by the following formulapose(v)：
Wherein:
kp2(v) a proportionality coefficient representing the posture deviation of the tail end of the dental implant surgery robot corresponding to the v-th degree of freedom;
kd2(v) a proportionality coefficient representing the variation rate of the posture deviation of the tail end of the dental implant surgery robot corresponding to the v-th degree of freedom;
step S6.5, thereby obtaining speed control quantities for six degrees of freedom, respectively: translation linear velocity V corresponding to 1 st degree of freedomposition(1) And the translation linear velocity V corresponding to the 2 nd degree of freedomposition(2) And the translation linear velocity V corresponding to the 3 rd degree of freedomposition(3) Corresponding to the 4 th degree of freedomAngular velocity V of rotationpose(4) Angular velocity V of rotation corresponding to the 5 th degree of freedompose(5) Angular velocity V of rotation corresponding to the 6 th degree of freedompose(6)；
And generating a speed control instruction by using the speed control quantity with six degrees of freedom, sending the speed control instruction to the tail end mechanical arm of the dental implant surgery robot, and controlling the tail end mechanical arm of the dental implant surgery robot with six degrees of freedom, thereby realizing the flexible control of the tail end of the dental implant surgery robot.
2. The method for controlling the compliance of the distal end of a dental implant surgical robot according to claim 1, further comprising: the compliance of the tail end of the dental implant surgery robot in six degrees of freedom and the response speed are adjusted in real time by adjusting the mass value of a mass block and/or the stiffness coefficient of a spring and/or the damping coefficient of a damper in a second-order mass-spring-damping system corresponding to each degree of freedom.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210167765.9A CN114505859B (en) | 2022-02-23 | 2022-02-23 | Method for controlling compliance of tail end of dental implant operation robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210167765.9A CN114505859B (en) | 2022-02-23 | 2022-02-23 | Method for controlling compliance of tail end of dental implant operation robot |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114505859A true CN114505859A (en) | 2022-05-17 |
| CN114505859B CN114505859B (en) | 2023-12-01 |
Family
ID=81552846
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210167765.9A Active CN114505859B (en) | 2022-02-23 | 2022-02-23 | Method for controlling compliance of tail end of dental implant operation robot |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114505859B (en) |
Citations (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102848391A (en) * | 2012-09-20 | 2013-01-02 | 北京邮电大学 | Four-channel bilateral teleoperation control system based on actual force feedback |
| CN104723340A (en) * | 2015-03-07 | 2015-06-24 | 哈尔滨工业大学 | Impedance control method for flexibility joint mechanical arm based on connection and damping configuration |
| US20160207201A1 (en) * | 2012-04-18 | 2016-07-21 | Massachusetts Institute Of Technology | Neuromuscular Model-Based Sensing And Control Paradigm For A Robotic Leg |
| CN110276806A (en) * | 2019-05-27 | 2019-09-24 | 江苏大学 | Online hand-eye calibration and crawl pose calculation method for four-freedom-degree parallel-connection robot stereoscopic vision hand-eye system |
| US20200269349A1 (en) * | 2019-02-25 | 2020-08-27 | Fanuc Corporation | Laser machine |
| US20200282558A1 (en) * | 2019-03-07 | 2020-09-10 | LinkDyn Robotics Inc. | System and method for controlling a robot with torque-controllable actuators |
| CN112809667A (en) * | 2020-12-22 | 2021-05-18 | 上海新时达机器人有限公司 | Force control method and device of industrial robot and application of force control device |
| US20210283771A1 (en) * | 2020-03-13 | 2021-09-16 | Omron Corporation | Control apparatus, robot, learning apparatus, robot system, and method |
| CN113400325A (en) * | 2021-06-23 | 2021-09-17 | 四川锋准机器人科技有限公司 | Navigation and positioning method for dental implant robot |
| CN113448246A (en) * | 2021-05-25 | 2021-09-28 | 上海交通大学 | Self-evolution posture adjustment method and system for oral implantation robot |
| CN113749804A (en) * | 2021-07-06 | 2021-12-07 | 上海优医基医疗影像设备有限公司 | Oral implant surgical robot with CT imaging system and control method thereof |
| CN113768640A (en) * | 2021-11-09 | 2021-12-10 | 极限人工智能有限公司 | Method and device for determining working pose of mechanical arm |
-
2022
- 2022-02-23 CN CN202210167765.9A patent/CN114505859B/en active Active
Patent Citations (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20160207201A1 (en) * | 2012-04-18 | 2016-07-21 | Massachusetts Institute Of Technology | Neuromuscular Model-Based Sensing And Control Paradigm For A Robotic Leg |
| CN102848391A (en) * | 2012-09-20 | 2013-01-02 | 北京邮电大学 | Four-channel bilateral teleoperation control system based on actual force feedback |
| CN104723340A (en) * | 2015-03-07 | 2015-06-24 | 哈尔滨工业大学 | Impedance control method for flexibility joint mechanical arm based on connection and damping configuration |
| US20200269349A1 (en) * | 2019-02-25 | 2020-08-27 | Fanuc Corporation | Laser machine |
| US20200282558A1 (en) * | 2019-03-07 | 2020-09-10 | LinkDyn Robotics Inc. | System and method for controlling a robot with torque-controllable actuators |
| CN110276806A (en) * | 2019-05-27 | 2019-09-24 | 江苏大学 | Online hand-eye calibration and crawl pose calculation method for four-freedom-degree parallel-connection robot stereoscopic vision hand-eye system |
| US20210283771A1 (en) * | 2020-03-13 | 2021-09-16 | Omron Corporation | Control apparatus, robot, learning apparatus, robot system, and method |
| CN112809667A (en) * | 2020-12-22 | 2021-05-18 | 上海新时达机器人有限公司 | Force control method and device of industrial robot and application of force control device |
| CN113448246A (en) * | 2021-05-25 | 2021-09-28 | 上海交通大学 | Self-evolution posture adjustment method and system for oral implantation robot |
| CN113400325A (en) * | 2021-06-23 | 2021-09-17 | 四川锋准机器人科技有限公司 | Navigation and positioning method for dental implant robot |
| CN113749804A (en) * | 2021-07-06 | 2021-12-07 | 上海优医基医疗影像设备有限公司 | Oral implant surgical robot with CT imaging system and control method thereof |
| CN113768640A (en) * | 2021-11-09 | 2021-12-10 | 极限人工智能有限公司 | Method and device for determining working pose of mechanical arm |
Non-Patent Citations (3)
| Title |
|---|
| 王文杰: "外科手术机器人位置控制和微器械力检测方法研究", 中国博士学位论文全文数据库 信息科技, no. 6 * |
| 邝宏武;郝矿荣;丁永生;: "机器人捕捉运动目标的时间最优控制方法", 机械设计与研究, no. 06 * |
| 阮晓钢;李世臻;侯旭阳;李欣源;: "基于非线性PID的柔性两轮机器人运动控制", 控制工程, no. 03 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114505859B (en) | 2023-12-01 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111660306B (en) | Robot variable admittance control method and system based on operator comfort | |
| US20200282558A1 (en) | System and method for controlling a robot with torque-controllable actuators | |
| US11850014B2 (en) | Control system, control method, and surgical arm system | |
| CN107028663A (en) | A kind of new master-slave mode operating robot control method | |
| JP7324932B2 (en) | dynamic planning controller | |
| Aboaf et al. | Living with the singularity of robot wrists | |
| CN111290269A (en) | Self-adaptive compliance stable control method of space robot | |
| CN113001540A (en) | Man-machine interaction intelligent control method of load-mobile exoskeleton and exoskeleton system | |
| CN115502966A (en) | Variable admittance control method for robot | |
| CN114102603A (en) | Zero-force dragging method based on Cartesian space | |
| CN114505859A (en) | Tail end smoothness control method for dental implant surgical robot | |
| Jägersand et al. | Acquiring visual-motor models for precision manipulation with robot hands | |
| CN116901057A (en) | Double mechanical arm tail end position cooperative control method based on variable impedance strategy | |
| JPS618236A (en) | Moving body control unit | |
| JPS60118478A (en) | Controller for position of joint type robot | |
| EP4375029A1 (en) | Drive device, method for controlling drive device, parallel link robot, and method for controlling parallel rink robot | |
| WO2023002642A1 (en) | Drive device, method for controlling drive device, parallel link robot, and method for controlling parallel rink robot | |
| Yaryan et al. | Modified PD based bilateral nonlinear teleoperation with flexible link robots | |
| CN116852397B (en) | Self-adaptive adjusting method for physiotherapy force and physiotherapy path of negative pressure physiotherapy robot | |
| JPH0780786A (en) | Impedance control device of robot | |
| Christoforou et al. | A passivity-based control case study of flexible-link manipulators | |
| Alikhani et al. | Modeling, control and simulation of a new large scale cable-driven robot | |
| Sam et al. | Calibration of 6 DOF stewart platform system using LabVIEW software | |
| Quach et al. | Visual based tracking of planar robot arms: a scheme using projection matrix | |
| CN117549305A (en) | Sliding mode-admittance-based modularized mechanical arm self-learning interaction control method |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |