CN113787524B

CN113787524B - Control method and device of mechanical arm, surgical robot and storage medium

Info

Publication number: CN113787524B
Application number: CN202111365458.3A
Authority: CN
Inventors: 王迎智; 杨猛; 张二虎
Original assignee: Jixian Artificial Intelligence Co Ltd
Current assignee: Jixian Artificial Intelligence Co Ltd
Priority date: 2021-11-18
Filing date: 2021-11-18
Publication date: 2022-02-15
Anticipated expiration: 2041-11-18
Also published as: CN113787524A

Abstract

The invention provides a control method and device of a mechanical arm, a surgical robot and a storage medium, and belongs to the technical field of biological medical treatment. According to the invention, when the distance between the car needle and the position of the prepared pit is far, the car needle is efficiently moved to the position near the position of the prepared pit by cooperating with the external force operation control mechanical arm of the medical worker, when the distance is near, the posture of the car needle is adjusted by cooperating with the external force operation control mechanical arm of the medical worker, and when the prepared pit is drilled, the car needle is drilled along the planning axial direction of the position of the prepared pit by cooperating with the external force operation control mechanical arm of the medical worker, so that the mechanical arm can accurately complete the preparation process of the implant pit hole by cooperating with the medical worker, and the efficiency of the implantation operation is improved.

Description

Control method and device of mechanical arm, surgical robot and storage medium

Technical Field

The invention belongs to the technical field of biomedical treatment, and particularly relates to a control method and device of a mechanical arm, a surgical robot and a storage medium.

Background

The dental implant operation is an operation of screwing an implant into a jaw bone of an oral cavity of a patient with missing teeth by using an implant device, namely, planting a cavity, and along with the development of an operation robot, the operation robot is applied to the operation of planting the cavity.

However, because the joints of the surgical robot interfere with each other, some links still need to be completed by medical staff because all links of the planting operation cannot be completed independently, so that the time of the planting operation is prolonged, and the efficiency of the planting operation is reduced.

Disclosure of Invention

The invention provides a control method and device of a mechanical arm, a surgical robot and a storage medium.

Some embodiments of the present invention provide a method for controlling a robot arm, applied to a controller of the robot arm, the method including:

acquiring the distance between a needle carried by the mechanical arm and a position of a standby socket of the implant through position detection equipment, and acquiring a pressure value applied to the mechanical arm through a pressure sensor;

when the distance is larger than a first distance threshold value, controlling the mechanical arm to move the needle to the position of the auxiliary socket at a first movement speed, wherein the first movement speed is in positive correlation with the distance and the pressure value and is smaller than or equal to a first speed threshold value, and an angular speed component of the first movement speed allows the needle to perform attitude adjustment in a movable range of the mechanical arm;

when the distance is smaller than or equal to the first distance threshold and larger than a second distance threshold, controlling the mechanical arm to move the needle to the position of the auxiliary pit at a second movement speed, wherein the second movement speed is in positive correlation with the distance and the pressure value, and an angular speed component of the second movement speed is smaller than that of the first movement speed, so as to limit the needle to be within a preset limit area in the movement process at the stage;

under the condition that the distance is less than the second distance threshold value, control the arm will the car needle is with third rate of motion along the planning axial of preparing the nest position and is equipped with the nest, third rate of motion with distance and vertical pressure value are positive correlation, vertical pressure value is the pressure value component of pressure value in car needle axis of rotation direction.

Optionally, the obtaining, by the position detection device, a distance between a needle carried by the mechanical arm and a socket position of the implant includes:

acquiring mark position information of the oral marking device in a coordinate system of the position detection device through the position detection device to represent the position information of the needle; and

calculating the prepared socket position information of the prepared socket position under the mechanical arm base coordinate system by utilizing a pre-calibrated coordinate system conversion relation and a pre-acquired position conversion relation between the oral cavity marking equipment and the prepared socket position;

and calculating the distance and the angle deviation based on the position information of the standby pit and the position information of the vehicle needle under the coordinate system of the mechanical arm base.

Optionally, before said controlling said robotic arm to move said needle at a first speed of motion towards said stock location, said method further comprises:

when the distance is greater than or equal to a third distance threshold, taking a first preset linear speed as a first linear speed, or when the distance is smaller than the third distance threshold, converting the distance into the first linear speed, wherein the third distance threshold is greater than the first distance threshold;

and when the angular deviation is greater than or equal to a first angular deviation threshold value, taking a first preset angular speed as the first angular speed, or when the angular deviation is less than the first angular deviation threshold value, converting the angular deviation into the first angular speed;

converting the pressure value into a first additional speed based on a preset first speed conversion parameter;

and combining the first linear speed, the first angular speed and the first additional speed to obtain the first movement speed.

Optionally, before the controlling the robotic arm to move the needle to the stock location at a second speed of motion, the method further comprises:

when the distance is greater than a fourth distance threshold, converting the distance into a second linear velocity, or when the distance is less than or equal to a fourth distance threshold, taking a second preset linear velocity as the second linear velocity, wherein the fourth distance threshold is greater than the second distance threshold and less than the first distance threshold;

and when the angle deviation is smaller than a second angle deviation threshold value, taking the second preset angle as a second angular velocity, or when the angle deviation is larger than or equal to the second angle deviation threshold value, converting the angle deviation into the second angular velocity;

and, converting the pressure value to a second additional speed based on a second speed conversion parameter, the second speed conversion parameter being related to the distance;

and combining the second linear velocity, the second angular velocity and the second additional velocity to obtain a second motion velocity, so as to control the mechanical arm to move the needle to the auxiliary socket position based on the second motion velocity.

Optionally, before the controlling the mechanical arm to drill the socket along the planned axial direction of the socket position at the third movement speed, the method further includes:

when the distance is greater than a fifth distance threshold, converting the distance into a third linear velocity, or when the distance is less than or equal to a fifth distance threshold, taking a third preset linear velocity as the third linear velocity, wherein the fifth distance threshold is less than the second distance threshold;

and when the angle deviation is smaller than a third angle deviation threshold value, taking the third preset angle as a third angular speed, or when the angle deviation is larger than or equal to the third angle deviation, converting the angle deviation into the third angular speed;

converting a component force of the pressure value in the direction perpendicular to the position of the auxiliary nest by the needle into a third additional speed based on a preset third speed conversion parameter;

and combining the third linear speed, the third angular speed and the third additional speed to obtain a third movement speed, so as to control the mechanical arm to move the needle to the position of the auxiliary socket based on the third movement speed.

Optionally, the controlling the robotic arm to move the needle toward the recoil position at a first movement speed if the distance is greater than a first distance threshold includes:

and controlling the mechanical arm to move the needle to the position of the auxiliary socket at a first movement speed under the condition that the distance is smaller than or equal to the first distance threshold and larger than a second distance threshold, and the distance is smaller than a first vertical distance threshold when a first vertical distance of the needle perpendicular to the implant direction is smaller than a first parallel distance threshold when a first parallel distance of the needle parallel to the implant direction is smaller than a first parallel distance threshold.

Optionally, in the case that the distance is smaller than the second distance threshold, controlling the mechanical arm to drill the needle along the planned axial direction of the stock pit position at a third movement speed to stock the pit, including:

and when the distance is smaller than or equal to the second distance threshold, and the distance is smaller than a second vertical distance threshold when a second vertical distance of the needle perpendicular to the direction of the auxiliary pit position is smaller than a second vertical distance threshold, and the distance is smaller than a second parallel distance threshold when a second parallel distance of the needle parallel to the direction of the auxiliary pit position is smaller than the second parallel distance threshold, controlling the mechanical arm to drill the needle at a third movement speed along the planned axial direction of the auxiliary pit position for auxiliary pit preparation, wherein the second vertical distance threshold is smaller than the first vertical distance threshold, and the second parallel distance threshold is smaller than the first parallel distance threshold.

Optionally, the position detection device comprises: an infrared camera, the oral marking apparatus comprising: the first infrared reflecting part is arranged outside the oral cavity, and the second infrared reflecting part is arranged at the tail end of the mechanical arm;

the acquiring, by a position detection device, the mark position information of an oral cavity marking device in a coordinate system of the position detection device to represent the needle position information includes:

and the marking position information of the oral cavity marking equipment in the coordinate system of the position detection equipment is obtained through the position detection equipment so as to represent the position information of the needle.

Optionally, after the acquiring, by the position detection device, the mark position information of the oral marking device in the coordinate system of the position detection device to characterize the needle position information, the method further comprises:

and adjusting the height value of the position of the prepared nest according to the preset height.

Some embodiments of the present invention provide a control apparatus for a robot arm, applied to a controller of the robot arm, the apparatus including:

the position detection module is configured to acquire the distance between a needle carried by the mechanical arm and a position of a standby socket of the implant through position detection equipment, and acquire a pressure value applied to the mechanical arm through a pressure sensor;

a control module configured to control the mechanical arm to move the needle to the standby position at a first movement speed when the distance is greater than a first distance threshold, wherein the first movement speed is positively correlated with the distance and the pressure value and is less than or equal to a first speed threshold, and an angular velocity component of the first movement speed allows the needle to perform attitude adjustment within a movable range of the mechanical arm;

Optionally, the position detection module is further configured to:

Optionally, the control module is further configured to:

the location detection module further configured to:

Optionally, the position detection module is further configured to:

Some embodiments of the invention provide a surgical robot comprising: memory, processor, robot arm and computer program stored on the memory and operable on the processor, the processor implementing the control method of the robot arm when executing the computer program

Some embodiments of the invention provide a computer program comprising computer readable code which, when run on a computing processing device, causes the computing processing device to perform a method of controlling a robotic arm as described above.

Some embodiments of the invention provide a non-transitory computer readable medium in which a control method of a robot arm as described above is stored.

According to the control method and device of the mechanical arm, the surgical robot and the storage medium provided by the invention, when the distance between the car needle and the position of the standby nest is far, the car needle is efficiently moved to the position near the position of the standby nest by cooperating with the external force operation control mechanical arm of the medical worker, when the distance is near, the posture of the car needle is adjusted by cooperating with the external force operation control mechanical arm of the medical worker, and when the standby nest is drilled, the car needle is drilled along the planning axial direction of the position of the standby nest by cooperating with the external force operation control mechanical arm of the medical worker, so that the mechanical arm can cooperate with the medical worker to accurately complete the preparation process of the implant nest hole, and the efficiency of the implant operation is improved.

The foregoing description is only an overview of the technical solutions of the present invention, and the embodiments of the present invention are described below in order to make the technical means of the present invention more clearly understood and to make the above and other objects, features, and advantages of the present invention more clearly understandable.

Drawings

In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below, and it is obvious that the drawings in the following description are some embodiments of the present invention, and those skilled in the art can also obtain other drawings according to the drawings without creative efforts.

Fig. 1 schematically illustrates a flow chart of a method of controlling a robotic arm provided by some embodiments of the present invention;

FIG. 2 schematically illustrates one of the principles of a method for controlling a robot according to some embodiments of the present invention;

FIG. 3 schematically illustrates a second schematic diagram of a method for controlling a robot according to some embodiments of the present invention;

FIG. 4 schematically illustrates a third schematic diagram of a method for controlling a robot according to some embodiments of the present invention;

FIG. 5 schematically illustrates a flow chart of a method of distance determination provided by some embodiments of the invention;

fig. 6 is a schematic structural diagram of a control device of a robot arm according to some embodiments of the present invention;

FIG. 7 schematically illustrates a block diagram of a computing processing device for performing methods according to some embodiments of the invention;

fig. 8 schematically illustrates a memory unit for holding or carrying program code implementing methods according to some embodiments of the invention.

Detailed Description

In order to make the objects, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are some, but not all, embodiments of the present invention. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.

Fig. 1 schematically shows a flow chart of a control method of a robot arm, which is applied to a controller of the robot arm, according to the present invention, and the method includes:

step 101, obtaining the distance between a needle carried by the mechanical arm and a position of a standby socket of the implant through position detection equipment, and obtaining a pressure value applied to the mechanical arm through a pressure sensor.

It should be noted that, referring to fig. 2, the robot arm 1122 may rotate around a joint to move the end needle 11221 disposed on the robot arm 1122. The position detection device is an electronic device with a position detection function, and can be independently arranged with the mechanical arm or integrally arranged on the mechanical arm. The pressure sensor may be a pressure sensor provided on a surface layer of the arm, or may be a force sensor provided inside the arm. The implant is a screw-like implant medical instrument for screwing into an implant cavity of the upper/lower jaw, over which an artificial crown is fitted. The position of the prepared pit is the position of the required vegetation planting pit in the oral cavity of the user.

In the embodiment of the invention, in the preparation process of the planting cavity, the controller of the mechanical arm can identify the position mark points for representing the positions of the needles through the position detection equipment in real time, and identify the position mark points for representing the positions of the spare cavities of the implant to obtain the positions of the needles and the positions of the spare cavities so as to calculate the distance between the positions of the needles and the positions of the spare cavities. And the controller of arm still obtains the pressure value that the arm received the external force from pressure sensor simultaneously, and this external force is that medical personnel applyed to the arm usually to make the arm can cooperate medical personnel preparation to plant the cave hole.

And 102, controlling the mechanical arm to move the needle to the position of the auxiliary socket at a first movement speed when the distance is greater than a first distance threshold, wherein the first movement speed is positively correlated with the distance and the pressure value and is less than or equal to the first speed threshold, and an angular speed component of the first movement speed allows the needle to perform attitude adjustment in a movable range of the mechanical arm.

The first distance threshold is the minimum distance between the needle and the auxiliary pocket position when the robot arm is freely adjusted within the movable range without restriction.

In an embodiment of the invention, referring to fig. 3, before the manipulator carries the stylus 11221 to the socket preparing position for drilling and socket preparing, the manipulator needs to perform posture adjustment on the stylus 11221 so that the stylus is aligned with the socket preparing position. Specifically, the controller of the mechanical arm adjusts the first movement speed in positive correlation with the pressure value of the external force applied to the mechanical arm from the medical staff, that is, the larger the pressure value is, the larger the first movement speed is, and otherwise, the smaller the pressure value is, the smaller the first movement speed is. And considering that the closer the distance between the positions of the auxiliary cells, the greater the influence of the moving speed of the needle on the adjustment efficiency of the needle, the smaller the distance, the greater the first movement speed, and vice versa, the greater the first movement speed, so as to limit the speed of the needle before reaching the position of the auxiliary cells. Considering that the influence of the needle on the user is small when the distance between the needle and the position of the standby socket is long, when the distance between the needle and the pressure value is larger than a first distance threshold value, the angular velocity component of the first movement velocity of the mechanical arm carrying the needle to move is not equal to zero when the mechanical arm is out of the movable range, so that the needle can be randomly adjusted in the movable range of the mechanical arm.

Step 103, when the distance is smaller than or equal to the first distance threshold and larger than a second distance threshold, controlling the mechanical arm to move the needle to the position of the auxiliary socket at a second movement speed, wherein the second movement speed is in positive correlation with the distance and the pressure value, and an angular velocity component of the second movement speed is smaller than that of the first movement speed, so as to limit that the needle is within a preset limit area in the movement process at this stage.

It should be noted that the second distance threshold is the minimum distance between the needle and the standby position when the robot arm can perform attitude adjustment. The preset limiting region refers to a region of the car needle, which is allowed to move outside the prepared socket position, and since the prepared socket position is in the oral cavity of the patient, the preset limiting region may be a region which is outside the prepared socket position and can accommodate the mechanical arm to carry the car needle to move, for example, a conical region or a cylindrical region in front of a plane where the oral cavity of the patient is located according to the prepared socket position principle, which may be specifically set according to actual requirements, and is not limited herein. A specific preset limiting area is given in the following embodiments of the present invention, please refer to the following detailed description of the motion phase related to step 103, which is not described in detail here for the time being.

In the embodiment of the invention, in the motion process at this stage, the fact that the distance between the needle and the position of the prepared pit is short is considered, so that the posture adjustment amplitude of the needle is limited by limiting the angular velocity component of the second motion speed, and particularly the angular velocity component of the second motion speed needs to be smaller than the minimum value of the angular velocity component of the first motion speed, so that the mechanical arm can cooperate with external force applied to the mechanical arm by medical personnel to more efficiently align the posture of the needle to the position of the prepared pit. The relationship between the second movement speed and the pressure value and distance is similar to the first movement speed, except that the angular velocity component of the second movement speed is limited, so that the second movement speed is slower than the first movement speed, and the linear velocity component of the second movement speed can also be smaller than the first movement speed.

After the movement process at this stage is finished, the needle can be determined to be or reach the position above the socket preparation position, and the posture of the needle is aligned with the planning axis of the socket preparation position.

And 104, under the condition that the distance is smaller than the second distance threshold value, controlling the mechanical arm to drill the vehicle needle at a third movement speed along the planned axial direction of the position of the prepared hole for preparing the hole, wherein the third movement speed is in positive correlation with the distance and a vertical pressure value, and the vertical pressure value is a pressure value component of the pressure value in the direction of the rotation shaft of the vehicle needle.

The planning axis of the position of the spare nest is the movement path of the needle for drilling the spare nest, and the movement path can be preset or can be automatically calculated by the controller according to the position of the spare nest.

In the embodiment of the present invention, referring to fig. 4, after the manipulator 11221 carried by the manipulator passes through the previous movement stage, the manipulator 11221 is already aligned with the planned axis of the standby socket position and the relative position is fixed, and at this time, the posture of the manipulator does not need to be adjusted, and the third movement speed of the manipulator only responds to the influence of the pressure value component of the external force in the direction of the rotation axis of the manipulator on the basis of the positive correlation with the distance, that is, the third movement speed only has a positive correlation with the vertical pressure value and the distance and is unrelated to the horizontal pressure value of the pressure value in the horizontal direction. Therefore, in the motion control stage, the mechanical arm can carry the vehicle needle to cooperate with medical staff to drill holes on the planning axis of the nest preparation position for preparing the nest hole.

According to the embodiment of the invention, when the distance between the car needle and the position of the standby nest is longer, the car needle is efficiently moved to the position close to the position of the standby nest by cooperating with the external force operation control mechanical arm of the medical worker, when the distance is shorter, the posture of the car needle is adjusted by cooperating with the external force operation control mechanical arm of the medical worker, and when the hole is drilled for standby nest, the car needle is drilled for standby nest along the planned axial direction of the position of the standby nest by cooperating with the external force operation control mechanical arm of the medical worker, so that the mechanical arm can cooperate with the medical worker to accurately complete the preparation process of the implant nest hole, and the efficiency of the implantation operation is improved.

Optionally, referring to fig. 5, the step 101 may include:

step 1011, acquiring the mark position information of the oral cavity mark device under the coordinate system of the position detection device through the position detection device to represent the position information of the needle.

It should be noted that the oral cavity marking device is an infrared marking point disposed outside the oral cavity of the patient, and the position detection device can identify the infrared marking point to acquire marking position information.

In an embodiment of the present invention, the calibration from the robot base coordinate system { base } to the robot end mark point coordinate system { tool } may be obtained by the robot controller according to the configuration file

. Before the auxiliary stage of the mechanical arm planting, the selected needle is calibrated, and the mechanical arm can acquire calibration from a controller to a coordinate system { tool } from a marking point coordinate system at the tail end of the mechanical arm to a needle coordinate system { tip }

. After planning the implant position, the controller calculates the calibration from the oral position coordinate system { oral } to the implant coordinate system { Implant }

. After the auxiliary stage of planting the mechanical arm, the controller of the mechanical arm is connected with the position detection equipment to obtain the relation between all the mark points, namely the calibration from the coordinate system { tool } of the mark point at the tail end of the mechanical arm to the coordinate system { oral } of the oral cavity position

This value will vary with the patient's mouth and the movement of the robotic arms.

Among the aforementioned inputs

、

、

As a fixed value, may be stored in the controller of the robot arm detected when the controller of the robot arm receives the position detection data

Then, the calibration from the mechanical arm base coordinate system to the machine needle coordinate system can be obtained through the calculation of the following formula (1)

：

（1）

Then to

The coordinate transformation is carried out to obtain the Euler angle of the machine needle

Cartesian coordinates of a hand-knitting machine

The transformation process may refer to the transformation method between the euler angles and the rotation matrices in the related art, and is not described herein again.

Step 1012, calculating the prepared socket position information of the prepared socket position under the robot arm base coordinate system by using the pre-calibrated coordinate system conversion relationship and the pre-acquired position conversion relationship between the oral cavity marking device and the prepared socket position.

In the embodiment of the invention, the calibration from the mechanical arm base coordinate system { base } to the implant coordinate system { Implant } can be obtained by calculating the following formula (2)

：

（2）

The calibration from the mechanical arm base coordinate system { base } to the standby pit position coordinate system { dst } is obtained through the calculation of the following formula (3)

：

（3）

Then to

Coordinate transformation is carried out to obtain the Euler angle of the position of the prepared pit

Cartesian coordinates of He cell position

。

And 1013, calculating the distance and the angle deviation based on the position information of the standby pit and the position information of the needle in the mechanical arm base coordinate system.

In the embodiment of the invention, the Cartesian coordinates of the needle can be calculated by the following formula (4)

Cartesian coordinates to the Standby nest position

Space cartesian distance therebetween

：

（4）

The angular deviation is the Euler angle of the sewing needle

And

difference of (2)

。

Optionally, before the step 102, the method further comprises:

a1, when the distance is larger than or equal to a third distance threshold value, taking a first preset linear speed as the first linear speed, or, when the distance is smaller than the third distance threshold value, converting the distance into the first linear speed, wherein the third distance threshold value is larger than the first distance threshold value.

In an embodiment of the present invention, to avoid that the linear velocity of the robot arm is too fast when the distance between the needle and the position of the auxiliary socket is too long, the first linear velocity may be limited to a first preset linear velocity when the distance is greater than or equal to the third distance threshold, and the first preset linear velocity may be a first linear velocity converted from the third distance threshold or less. Assuming that the third distance threshold is 100, the first linear velocity can be calculated by the following equation (5)

：

（5）

Of course, the above description is only an exemplary illustration, and the third distance threshold and the first preset linear velocity may be specifically set according to actual requirements, which is not limited herein.

And A2, when the angle deviation is larger than or equal to a first angle deviation threshold value, taking a first preset angular speed as the first angular speed, or when the angle deviation is smaller than the first angle deviation threshold value, converting the angle deviation into the first angular speed.

In an embodiment of the present invention, to avoid that the angular velocity of the mechanical arm is too fast when the distance between the needle and the position of the cradle is too long, when the angular deviation is greater than or equal to the first angular deviation threshold, the first angular velocity may be limited to a first predetermined angular velocity, which may be a first angular velocity converted from the first angular deviation threshold or less. Assuming that the first angular deviation threshold is 10, the first linear velocity can be calculated by the following equation (6)

：

（6）

Of course, the above description is only an exemplary illustration, and the first angular deviation threshold and the first preset angular speed may be specifically set according to actual requirements, which is not limited herein.

A3, converting the pressure value into a first additional speed based on a preset first speed conversion parameter.

In the embodiment of the invention, when the controller of the mechanical arm detects the pressure value of the mechanical arm operated by the external force applied by the medical staff

Then, the first additional speed can be obtained by converting the following equation (7)

：

（7）

The K1 and K2 are first speed conversion parameters, and may be 20, 25, and the like, and specifically may be default settings of the controller, or may be self-set by the user, which is not limited herein.

And A4, combining the first linear speed, the first angular speed and the first additional speed to obtain the first movement speed.

In the embodiment of the present invention, the first movement velocity is calculated by the following formula (8)

：

（8）

Optionally, before step 103, the method further comprises:

b1, when the distance is greater than a fourth distance threshold, converting the distance into a second linear velocity, or when the distance is less than or equal to a fourth distance threshold, taking a second preset linear velocity as the second linear velocity, wherein the fourth distance threshold is greater than the second distance threshold and less than the first distance threshold.

In the embodiment of the present invention, in order to avoid that the linear velocity of the mechanical arm is too fast when the distance between the needle and the position of the auxiliary socket is too short, when the distance is less than or equal to the fourth distance threshold, the second linear velocity may be limited to the second preset linear velocity, and the second preset linear velocity may be the second linear velocity converted from the fourth distance threshold. Assuming that the fourth distance threshold is 30, the second linear velocity can be calculated by the following equation (9)

：

（9）

Of course, the above description is only an exemplary illustration, and the fourth distance threshold and the second preset linear velocity may be specifically set according to actual requirements, which is not limited herein.

B2, when the angle deviation is smaller than a second angle deviation threshold value, the second preset angle is used as a second angular velocity, or when the angle deviation is larger than or equal to the second angle deviation threshold value, the angle deviation is converted into the second angular velocity.

In an embodiment of the present invention, to avoid that the angular velocity of the mechanical arm is too slow when the distance between the needle and the position of the auxiliary socket is too close, when the angular deviation is smaller than the first angular deviation threshold, the first angular velocity may be limited to a second predetermined angular velocity, which may be a second angular velocity converted by being greater than or equal to the second angular deviation threshold. Assuming that the second angular deviation threshold is 5, the first linear velocity can be calculated by the following equation (10)

：

（10）

Of course, the above description is only an exemplary illustration, and the second angular deviation threshold and the second preset angular velocity may be specifically set according to actual requirements, which is not limited herein.

And B3, converting the pressure value into a second additional speed based on a second speed conversion parameter, wherein the second speed conversion parameter is related to the distance.

Then, the first additional speed can be obtained by converting the following equation (11)

：

（11）

The K3 and K4 are second speed conversion parameters, and may be 20, 25, and the like, and specifically may be default settings of the controller, or may be self-set by the user, which is not limited herein.

And B4, combining the second linear velocity, the second angular velocity and the second additional velocity to obtain a second motion velocity, and controlling the mechanical arm to move the needle to the auxiliary pit position based on the second motion velocity.

In the embodiment of the present invention, the first movement velocity is calculated by the following formula (12)

：

Optionally, before the step 104, the method further comprises:

and C1, when the distance is greater than a fifth distance threshold, converting the distance into a third linear velocity, or when the distance is less than or equal to a fifth distance threshold, taking a third preset linear velocity as the third linear velocity, wherein the fifth distance threshold is less than the second distance threshold.

In the embodiment of the present invention, in order to avoid that the linear velocity of the mechanical arm is too fast when the distance between the needle and the position of the auxiliary socket is too short, when the distance is less than or equal to the fifth distance threshold, the third linear velocity may be limited to a third preset linear velocity, which may be a third linear velocity converted from a value greater than the fifth distance threshold. Assuming that the fifth distance threshold is 30, the third linear velocity can be calculated by the following equation (13)

：

（13）

Of course, the above description is only an exemplary illustration, and the fifth distance threshold and the third preset linear velocity may be specifically set according to actual requirements, which is not limited herein.

And C2, when the angle deviation is smaller than a third angle deviation threshold value, taking the third preset angle as a third angular speed, or when the angle deviation is larger than or equal to the third angle deviation, converting the angle deviation into the third angular speed.

In an embodiment of the invention, the angular velocity of the robotic arm is only responsive to angular deviations from the sensor. In order to avoid that the angular speed of the mechanical arm is too slow when the distance between the needle and the position of the auxiliary socket is too close, when the angular deviation is smaller than a first angular deviation threshold value, the first angular speed can be limited to a second preset angular speed, and the second preset angular speed can be a second angular speed converted by being larger than or equal to a second angular deviation threshold value. Assuming that the second angular deviation threshold is 5, the first linear velocity can be calculated by the following equation (14)

：

（14）

Of course, the above description is only an exemplary illustration, and the third angular deviation threshold and the third preset angular velocity may be specifically set according to actual requirements, which is not limited herein.

And C3, converting the component force of the pressure value in the direction perpendicular to the position of the auxiliary socket by the needle into a third additional speed based on a preset third speed conversion parameter.

In the embodiment of the present invention, the z-axis of the needle coordinate system { tip } must be completely aligned with the z-axis of the stock-pit position coordinate system { dst } until the Euler angle of the stock-pit position after the needle reaches the stock-pit position coordinate system { dst }

Euler angle of the hand-sewing needle position

Satisfies the following formula (15):

（15）

it is determined that the needle coordinate system { tip } and the back-up pit position coordinate system { dst } are properly aligned, and a third additional velocity can be calculated as shown in equation (16) below

：

（16）

Where K is a second speed conversion parameter, and may be 20, 25, and the like, and specifically may be a default setting of the controller, or may be a setting set by the user, which is not limited herein.

If it is

Euler angle of the hand-sewing needle position

Satisfying the above equation (15), the third additional speed

。

And C4, combining the third linear speed, the third angular speed and the third additional speed to obtain a third movement speed, and controlling the mechanical arm to move the needle to the auxiliary pit position based on the third movement speed.

In the embodiment of the present invention, the third additional speed is calculated by the following equation (17):

（17）

as described above, in the motion stage, only the external force along the z-axis direction in the implant coordinate system { Implant } is responded, and the purpose of drilling with high precision of the mechanical arm is achieved.

Optionally, the step 103 may include: and controlling the mechanical arm to move the needle to the position of the auxiliary socket at a first movement speed under the condition that the distance is smaller than or equal to the first distance threshold and larger than a second distance threshold, and the distance is smaller than a first vertical distance threshold when a first vertical distance of the needle perpendicular to the implant direction is smaller than a first parallel distance threshold when a first parallel distance of the needle parallel to the implant direction is smaller than a first parallel distance threshold.

In the embodiment of the present invention, since the adjustment of the position and posture of the mechanical arm requires time, it is necessary to set the motion phase of step 103 for protection, in which the vehicle is aimed at the prepared nest position of the implant, the oral cavity of a normal adult can be generally opened to 50-60mm, and for safety, the height of the aligned cone region is set to 30mm, and the cone angle is set to 45 °. The tapered zone described herein is a predetermined restriction zone in one possible embodiment of the invention. At this time, the Cartesian coordinates of the hand can be calculated by the following formula (18)

Cartesian coordinates to reserve nest

First vertical distance of z-axis:

（18）

and calculating the Standby Cartesian coordinates by the following equation (19)

To the origin of the foveal position coordinate system { dst }:

（19）

if the first vertical distance threshold is 30 and the first horizontal distance threshold is 30, if

Then the motion phase of step 103 is entered, and at the end of this phase

。

In summary, assuming that the first distance threshold is 45, the second distance threshold is 0, the third distance threshold is 300, the fourth distance threshold and the fifth distance threshold are 0, the first vertical distance and the second vertical distance are 30, and the first horizontal distance and the second horizontal distance are 30, the method for dividing the regions in the three motion phases may be as follows:

optionally, the step 104 may include: and when the distance is smaller than or equal to the second distance threshold, and the distance is smaller than a second vertical distance threshold when a second vertical distance of the needle perpendicular to the direction of the auxiliary pit position is smaller than a second vertical distance threshold, and the distance is smaller than a second parallel distance threshold when a second parallel distance of the needle parallel to the direction of the auxiliary pit position is smaller than the second parallel distance threshold, controlling the mechanical arm to drill the needle at a third movement speed along the planned axial direction of the auxiliary pit position for auxiliary pit preparation, wherein the second vertical distance threshold is smaller than the first vertical distance threshold, and the second parallel distance threshold is smaller than the first parallel distance threshold.

In the embodiment of the present invention, the second vertical distance threshold and the second horizontal distance threshold are similar to the first vertical distance threshold and the first vertical distance threshold in the calculation manner, and are not described herein again.

In order to ensure the safety of the patient, the punching control mode can be entered only by the external operation of the medical staff, otherwise, the mechanical arm is always in the last two stages. When the mechanical arm controller detects that medical personnel pushes down the end tool towards the drilling direction, the mechanical arm can generate downward movement, and when the second vertical distance is smaller than the second vertical distance, the mechanical arm is controlled to carry the vehicle needle to drill holes along the planning axial direction of the position of the nest for the nest preparation. Generally, the second parallel distance threshold and the second perpendicular distance threshold may be 0, or may be other values that may indicate that drilling is to be started, and are not limited herein.

the step of acquiring, by the position detection device, the mark position information of the oral cavity marking device in the coordinate system of the position detection device to represent the needle position information may include: and the marking position information of the oral cavity marking equipment in the coordinate system of the position detection equipment is obtained through the position detection equipment so as to represent the position information of the needle.

In the embodiment of the disclosure, because there are demands for positioning the position of the implant and tracking the movement of the oral cavity of the patient, a first infrared light reflecting part is placed at the oral cavity of the patient for positioning the patient, and the position of the needle and the position of the prepared socket of the implant are finally positioned by converting the relationship of a coordinate system based on the position information of a second infrared light reflecting part at the tail end of the mechanical arm. For a specific calculation manner, reference may be made to the detailed description of step 1011 to step 1013, which is not described herein again.

Optionally, after the step 1012, the method further comprises: and adjusting the height value of the position of the prepared nest according to the preset height.

In the embodiments of the present inventionIn the method, the aim of the automatic deviation adjustment of the mechanical arm is not at the tip of the implant and needs to be at

On the basis, the length of the offset implant along the z-axis of the implant coordinate system { Implant } coordinate system is added with the distance of preset height values such as 10mm and 15mm, of course, the selected implant is not necessarily the same according to different conditions of patients and medical staff, but the length of the implant can be known from the main control trolley, so that the accuracy of the determined position of the preparation nest is ensured.

Fig. 6 schematically shows a structural diagram of a control device 20 of a robot arm, which is applied to a controller of the robot arm, according to the present invention, and the device includes:

the position detection module 201 is configured to acquire a distance between a needle carried by the mechanical arm and a position of a standby socket of the implant through position detection equipment, and acquire a pressure value received by the mechanical arm through a pressure sensor;

a control module 202 configured to control the mechanical arm to move the needle to the standby position at a first moving speed when the distance is greater than a first distance threshold, wherein the first moving speed is positively correlated with the distance and the pressure value and is less than or equal to a first speed threshold, and an angular velocity component of the first moving speed allows the needle to perform attitude adjustment within a movable range of the mechanical arm;

Optionally, the position detecting module 201 is further configured to:

Optionally, the control module 202 is further configured to:

the position detection module 201 is further configured to:

Optionally, the position detecting module 201 is further configured to:

According to the embodiment of the invention, when the distance between the car needle and the position of the standby nest is far, the car needle is efficiently moved to the position near the position of the standby nest by cooperating with the external force operation control mechanical arm of the medical worker, when the distance is near, the posture of the car needle is adjusted by cooperating with the external force operation control mechanical arm of the medical worker, and when the standby nest is drilled, the car needle is drilled along the planned axial direction of the position of the standby nest by cooperating with the external force operation control mechanical arm of the medical worker, so that the mechanical arm can accurately complete the preparation process of the implant nest hole by cooperating with the medical worker, and the efficiency of the implantation operation is improved.

The above-described embodiments of the apparatus are merely illustrative, and the units described as separate parts may or may not be physically separate, and parts displayed as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the solution of the present embodiment. One of ordinary skill in the art can understand and implement it without inventive effort.

The various component embodiments of the invention may be implemented in hardware, or in software modules running on one or more processors, or in a combination thereof. Those skilled in the art will appreciate that a microprocessor or Digital Signal Processor (DSP) may be used in practice to implement some or all of the functionality of some or all of the components in a computing processing device according to embodiments of the present invention. The present invention may also be embodied as apparatus or device programs (e.g., computer programs and computer program products) for performing a portion or all of the methods described herein. Such programs implementing the present invention may be stored on non-transitory computer readable media, or may be in the form of one or more signals. Such a signal may be downloaded from an internet website or provided on a carrier signal or in any other form.

For example, FIG. 7 illustrates a computing processing device in which a method in accordance with the present invention may be implemented. The computing processing device conventionally includes a processor 310 and a computer program product or non-transitory computer-readable medium in the form of a memory 320. The memory 320 may be an electronic memory such as a flash memory, an EEPROM (electrically erasable programmable read only memory), an EPROM, a hard disk, or a ROM. The memory 320 has a storage space 330 for program code 331 for performing any of the method steps of the above-described method. For example, the storage space 330 for the program code may include respective program codes 331 respectively for implementing various steps in the above method. The program code can be read from or written to one or more computer program products. These computer program products comprise a program code carrier such as a hard disk, a Compact Disc (CD), a memory card or a floppy disk. Such a computer program product is typically a portable or fixed storage unit as described with reference to fig. 8. The memory unit may have memory segments, memory spaces, etc. arranged similarly to memory 320 in the computing processing device of fig. 7. The program code may be compressed, for example, in a suitable form. Typically, the memory unit comprises computer readable code 331', i.e. code that can be read by a processor, such as 310, for example, which when executed by a computing processing device causes the computing processing device to perform the steps of the method described above.

It should be understood that, although the steps in the flowcharts of the figures are shown in order as indicated by the arrows, the steps are not necessarily performed in order as indicated by the arrows. The steps are not performed in the exact order shown and may be performed in other orders unless explicitly stated herein. Moreover, at least a portion of the steps in the flow chart of the figure may include multiple sub-steps or multiple stages, which are not necessarily performed at the same time, but may be performed at different times, which are not necessarily performed in sequence, but may be performed alternately or alternately with other steps or at least a portion of the sub-steps or stages of other steps.

Reference herein to "one embodiment," "an embodiment," or "one or more embodiments" means that a particular feature, structure, or characteristic described in connection with the embodiment is included in at least one embodiment of the invention. Moreover, it is noted that instances of the word "in one embodiment" are not necessarily all referring to the same embodiment.

In the description provided herein, numerous specific details are set forth. It is understood, however, that embodiments of the invention may be practiced without these specific details. In some instances, well-known methods, structures and techniques have not been shown in detail in order not to obscure an understanding of this description.

In the claims, any reference signs placed between parentheses shall not be construed as limiting the claim. The word "comprising" does not exclude the presence of elements or steps not listed in a claim. The word "a" or "an" preceding an element does not exclude the presence of a plurality of such elements. The invention may be implemented by means of hardware comprising several distinct elements, and by means of a suitably programmed computer. In the unit claims enumerating several means, several of these means may be embodied by one and the same item of hardware. The usage of the words first, second and third, etcetera do not indicate any ordering. These words may be interpreted as names.

Finally, it should be noted that: the above examples are only intended to illustrate the technical solution of the present invention, but not to limit it; although the present invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some technical features may be equivalently replaced; and such modifications or substitutions do not depart from the spirit and scope of the corresponding technical solutions of the embodiments of the present invention.

Claims

1. A method of controlling a robot arm, applied to a controller of the robot arm, the method comprising:

acquiring the distance and the angle deviation between a needle carried by the mechanical arm and a position of a standby socket of the implant through position detection equipment, and acquiring a pressure value applied to the mechanical arm through a pressure sensor;

when the distance is smaller than or equal to the first distance threshold and larger than a second distance threshold, controlling the mechanical arm to move the needle to the auxiliary pit position at a second movement speed, wherein the second movement speed is in positive correlation with the distance and the pressure value, and an angular speed component of the second movement speed is smaller than that of the first movement speed, so as to limit the needle to be within a preset limit area in the movement process of the current stage, and the second movement speed is related to the angular deviation;

2. The method according to claim 1, wherein the obtaining of the distance and angular deviation between the needle carried by the robotic arm and the seed position of the implant by the position detection device comprises:

calculating the distance and the angle deviation based on the position information of the standby pit and the position information of the vehicle needle under the coordinate system of the mechanical arm base;

and the angle deviation is the difference value of the Euler angle of the sewing needle and the Euler angle of the standby pit position.

3. The method of claim 2, wherein prior to said controlling said robotic arm to move said needle at a first speed of motion toward said pocket location, said method further comprises:

4. The method of claim 2, wherein prior to said controlling said robotic arm to move said needle at a second speed of motion toward said pocket location, said method further comprises:

5. The method of claim 2, wherein prior to said controlling said robotic arm to drill said needle in a drilling well preparation along a planned axis of a well preparation location at a third speed of motion, said method further comprises:

6. The method of claim 1, wherein controlling the robotic arm to move the needle toward the reseating location at a first speed of movement if the distance is greater than a first distance threshold comprises:

7. The method of claim 6, wherein in the event that the distance is less than the second distance threshold, controlling the robotic arm to drill the stylus along a planned axis of a standby position at a third speed of motion comprises:

8. The method of claim 2, wherein the location detection device comprises: an infrared camera, the oral marking apparatus comprising: the first infrared reflecting part is arranged outside the oral cavity, and the second infrared reflecting part is arranged at the tail end of the mechanical arm;

9. The method of claim 2, wherein after said obtaining, by a position detection device, marker position information of an oral marker device in a coordinate system of the position detection device to characterize the needle position information, the method further comprises:

10. A control apparatus of a robot arm, characterized by being applied to a controller of the robot arm, the apparatus comprising:

the position detection module is configured to acquire the distance and the angle deviation between a needle carried by the mechanical arm and a position of a standby socket of the implant through position detection equipment, and acquire a pressure value applied to the mechanical arm through a pressure sensor;

11. The apparatus of claim 10, wherein the location detection module is further configured to:

12. The apparatus of claim 11, wherein the control module is further configured to:

13. The apparatus of claim 11, wherein the control module is further configured to:

14. The apparatus of claim 11, wherein the control module is further configured to:

15. The apparatus of claim 10, wherein the control module is further configured to:

16. The apparatus of claim 15, wherein the control module is further configured to:

17. The apparatus of claim 11, wherein the position detection device comprises: an infrared camera, the oral marking apparatus comprising: the first infrared reflecting part is arranged outside the oral cavity, and the second infrared reflecting part is arranged at the tail end of the mechanical arm;

the location detection module further configured to:

18. The apparatus of claim 11, wherein the location detection module is further configured to:

19. A surgical robot comprising a memory, a processor, a robotic arm, and a computer program stored on the memory and executable on the processor, the processor implementing the method of controlling the robotic arm of any one of claims 1 to 9 when executing the computer program.

20. A computer-readable storage medium, characterized in that a computer program is stored thereon, which when executed by a processor implements the control method of a robot arm according to any one of claims 1 to 9.