METHOD AND SYSTEM FOR PREVENTING COLLISION BETWEEN MECHANICAL ARMS, AND MEDICAL ROBOT
20220015846 · 2022-01-20
Inventors
- Zihan Li (Shanghai, CN)
- Jiayin WANG (Shanghai, CN)
- Chao He (Shanghai, CN)
- Feijian NI (Shanghai, CN)
- Peng Zhang (Shanghai, CN)
- Yuhui XIA (Shanghai, CN)
Cpc classification
A61B90/03
HUMAN NECESSITIES
B25J9/1676
PERFORMING OPERATIONS; TRANSPORTING
A61B2034/302
HUMAN NECESSITIES
B25J9/0084
PERFORMING OPERATIONS; TRANSPORTING
G05B2219/39135
PHYSICS
B25J9/1607
PERFORMING OPERATIONS; TRANSPORTING
A61B34/76
HUMAN NECESSITIES
A61B34/70
HUMAN NECESSITIES
A61B2090/064
HUMAN NECESSITIES
G05B2219/40184
PHYSICS
G05B2219/45123
PHYSICS
International classification
A61B34/00
HUMAN NECESSITIES
B25J9/02
PERFORMING OPERATIONS; TRANSPORTING
Abstract
A method and system for preventing a collision between mechanical arms (21), and a medical robot, belonging to the field of medical robot technology. The method includes: arranging (S10) discrete points (m, n) at a mechanical arm (21); acquiring (S40) an interaction force (F.sub.m,n) corresponding to each discrete point (m, n) according to a calculated relative distance (L) between the discrete points (m, n) respectively on different mechanical arms (21), to obtain (S50) a resultant force of the interaction forces (F.sub.m,n) each of which corresponds to each discrete point (m, n), and then obtaining a Cartesian force (F.sub.d) corresponding to each mechanical arm (21), and making (S60) an operator perceive the Cartesian force (F.sub.d) in real time, thereby effectively reducing the risk of interference and collision between the mechanical arms (21).
Claims
1. A method for preventing a collision between mechanical arms, applied to a medical robot, the medical robot comprising at least two mechanical arms, a mechanical arm of the at least two mechanical arms comprising a mechanical arm body, the mechanical arm body comprising a plurality of joints, the method comprising: arranging a plurality of discrete points on each mechanical arm; acquiring first coordinate information of each discrete point in a global coordinate system; obtaining a first relative distance between two discrete points located on different mechanical arms according to the first coordinate information; acquiring a first interaction force at each discrete point applied by each of discrete points on the other mechanical arm according to the first relative distance and a corresponding relationship between the first relative distance and the first interaction force; and obtaining a Cartesian force of each mechanical arm according to a resultant force of first interaction forces each of which corresponds to each discrete point and each of discrete points on the other mechanical arm, and making the Cartesian force perceived by an operator.
2. The method according to claim 1, wherein at least a part of the mechanical arms further comprises a medical instrument mounted on a distal end of the mechanical arm body, the arranging the plurality of discrete points on each mechanical arm comprises arranging the discrete points on a medical instrument and/or the mechanical arm body.
3. The method according to claim 1, wherein the arranging the plurality of discrete points on each mechanical arm comprises: acquiring first relative position information of a discrete point with respect to a corresponding joint.
4. The method according to claim 3, wherein the mechanical arm further comprises a position sensor configured to acquire joint position information, the acquiring the first coordinate information of each discrete point in the global coordinate system comprises: obtaining the joint position information of the joint by the position sensor, and obtaining second coordinate information of each joint in the global coordinate system according to the joint position information and a kinematic model; and obtaining the first coordinate information of the discrete point in the global coordinate system according to the second coordinate information and the first relative position information.
5. The method according to claim 1, wherein the acquiring the first interaction force corresponding to each discrete point and each of discrete points on the other mechanical arm according to the first relative distance and the corresponding relationship between the first relative distance and the first interaction force comprises: setting a plurality of distance intervals in a value range of the first relative distance, wherein the corresponding relationship corresponding to at least one of the distance intervals is different from corresponding relationships corresponding to other distance intervals; and obtaining the first interaction force corresponding to each discrete point and each of discrete points on the other mechanical arm according to the first relative distance between the discrete points and the corresponding relationship corresponding to a distance interval which the first relative distance is in.
6. The method according to claim 5, wherein the plurality of distance intervals includes a first distance interval (0, L.sub.min+R.sub.m+R.sub.n], a second distance interval (L.sub.min+R.sub.m+R.sub.n, L.sub.max+R.sub.m+R.sub.n), and a third distance interval [L.sub.max+R.sub.m+R.sub.n, +∞); and the first interaction force F is a preset maximum interaction force F.sub.max in the first distance interval (0, L.sub.min+R.sub.m+R.sub.n]; a relationship between the first interaction force and the first relative distance is a function in the second distance interval (L.sub.min+R.sub.m+R.sub.n, L.sub.max+R.sub.m+R.sub.n), the function is a continuous function in the second distance interval, a first derivative of the function is less than zero, and a second derivative of the function is greater than zero; the first interaction force F is equal to zero in the third distance interval [L.sub.max+R.sub.m+R.sub.n, +∞); wherein discrete points m and n are two discrete points on different mechanical arms, R.sub.m is a cross-sectional maximum radius of a mechanical arm at the discrete point m, with the discrete point m located on the mechanical arm; R.sub.n is a cross-sectional maximum radius of a mechanical arm at the discrete point n, with the discrete point n located on the mechanical arm; L.sub.max is a preset collision warning distance of the first relative distance, and L.sub.min is a preset minimum safe distance of the first relative distance; wherein the function is:
7. (canceled)
8. The method according to claim 3, wherein the arranging the plurality of discrete points on each mechanical arm further comprises: acquiring a discrete point gain coefficient of each discrete point; wherein the obtaining the Cartesian force of each mechanical arm according to the resultant force of the first interaction forces each of which corresponds to each discrete point and each of discrete points on the other mechanical arm comprises: obtaining a resultant force of all first interaction forces corresponding to each discrete point; obtaining a torque corresponding to each discrete point with respect to a corresponding joint according to the resultant force of all the first interaction forces corresponding to each discrete point and the first relative position information of each discrete point with respect to a corresponding joint, and obtaining a resultant torque corresponding to each joint according to a discrete point gain coefficient of each discrete point; and obtaining the Cartesian force of the mechanical arm according to the resultant torque corresponding to each joint and a force Jacobian matrix of the mechanical arm.
9. The method according to claim 1, wherein the making the Cartesian force perceived by the operator comprises: displaying the Cartesian force by a display device.
10. The method according to claim 1, wherein the mechanical arm is connected to a master manipulator configured to control a movement of the mechanical arm, the making the Cartesian force perceived by the operator comprises: obtaining a Cartesian force of the master manipulator configured to control the movement of the mechanical arm according to the Cartesian force of the mechanical arm; obtaining a torque corresponding to each joint of the master manipulator according to a force Jacobian matrix and the Cartesian force of the master manipulator; and causing a motor configured to control a movement of a joint of the master manipulator to correspondingly output a resistance torque having a same magnitude as but an opposite direction to the torque corresponding to each joint of the master manipulator.
11. A system for preventing a collision between mechanical arms, applied to a medical robot, the medical robot comprising at least two mechanical arms, a mechanical arm comprising a mechanical arm body, the mechanical arm body comprising a plurality of joints, the system comprising: a discrete point arranging unit configured to arrange a discrete point on the mechanical arm; a memory configured to store instructions; a processor in communication with the discrete point arranging unit and the memory, respectively; wherein the instructions stored in the memory, when executed by the processor, perform the steps of: acquiring first coordinate information of each discrete point in a global coordinate system; obtaining a first relative distance between two discrete points on different mechanical arms according to the first coordinate information; acquiring a first interaction force at each discrete point applied by each of discrete points on the other mechanical arm according to the first relative distance and a corresponding relationship between the first relative distance and the first interaction force; and obtaining a Cartesian force of each mechanical arm according to a resultant force of first interaction forces each of which corresponds to each discrete point and each of discrete points on the other mechanical arm, and making the Cartesian force perceived by an operator.
12. The system according to claim 11, wherein at least a part of the mechanical arms further comprises a medical instrument mounted on a distal end of the mechanical arm body, and the discrete point is arranged on the medical instrument and/or the mechanical arm body.
13. The system according to claim 11, wherein, the mechanical arm further comprises a position sensor configured to acquire joint position information, the position sensor being in communication with the processor; the discrete point arranging unit is further configured to acquire first relative position information of the discrete point with respect to a corresponding joint; the instructions stored on the memory, when executed by the processor, further perform the steps of: acquiring, by the position sensor, joint position information of the joint, and acquiring second coordinate information of each joint in the global coordinate system according to the joint position information and a kinematic model; and acquiring the first coordinate information of each discrete point in the global coordinate system according to the second coordinate information of each joint in the global coordinate system and the first relative position information.
14. The system according to claim 11, wherein, the corresponding relationship between the first relative distance and the first interaction force is pre-stored in the memory; or, the system further comprises an input device configured to acquire the corresponding relationship between the first relative distance and the first interaction force during a surgery, to directly be executed by the processor or stored in the memory.
15. The system according to claim 11, wherein a value range of the first relative distance includes a plurality of distance intervals, and the corresponding relationship corresponding to at least one of the distance intervals is different from corresponding relationships corresponding to other distance intervals; wherein the instructions stored on the memory, when executed by the processor, further perform the steps of: obtaining a first interaction force corresponding to each discrete point and each of discrete points on the other mechanical arm according to the first relative distance of each discrete point and a corresponding relationship corresponding to a distance interval which the first relative distance is in.
16. The system according to claim 15, wherein the preset plurality of distance intervals comprise a first distance interval (0, L.sub.min+R.sub.m+R.sub.n], a second distance interval (L.sub.min+R.sub.m+R.sub.n, L.sub.max+R.sub.m+R.sub.n), and a third distance interval [L.sub.max+R.sub.n+R.sub.n, +∞); and the first interaction force F is a preset maximum interaction force F.sub.max in the first distance interval (0, L.sub.min+R.sub.m+R.sub.n]; a relationship between the first interaction force and the first relative distance is a function in the second distance interval (L.sub.min+R.sub.m+R.sub.n, L.sub.max+R.sub.m+R.sub.n), the function is a continuous function in the second distance interval, and a first derivative of the function is less than zero, and a second derivative of the function is greater than zero; the first interaction force F is zero in the third distance interval [L.sub.max+R.sub.m+R.sub.n, +∞); wherein discrete points m and n are two discrete points on different mechanical arms, R.sub.m is a cross-sectional maximum radius of a mechanical arm at the discrete point m, with the discrete point m located on the mechanical arm; R.sub.n is a cross-sectional maximum radius of a mechanical arm at the discrete point n, with the discrete point n located on the mechanical arm; L.sub.max is a preset collision warning distance of the first relative distance, and L.sub.min is a preset minimum safe distance of the first relative distance; wherein the function is:
17. (canceled)
18. The system according to claim 11, wherein the discrete point arranging unit is further configured to obtain a discrete point gain coefficient of each discrete point; the instructions stored in the memory, when executed by the processor, further perform steps of: obtaining a resultant force of first interaction forces each of which corresponds to each discrete point and each of discrete points on the other mechanical arm; obtaining a torque corresponding to each discrete point with respect to a corresponding joint according to the resultant force of the first interaction forces corresponding to each discrete point and the first relative position information of each discrete point with respect to a corresponding joint, and obtaining a resultant torque corresponding to each joint according to the torque corresponding to the discrete point with respect to the corresponding joint and the discrete point gain coefficient of each discrete point; and obtaining a Cartesian force of the mechanical arm according to the resultant torque corresponding to each joint and a force Jacobian matrix of the mechanical arm.
19. The system according to claim 11, further comprising an alarm connected to the processor, wherein the instructions stored on the memory, when executed by the processor, further perform steps of: when a first relative distance of any discrete point on the mechanical arm is less than or equal to a preset warning distance, and/or when the Cartesian force of the mechanical arm is greater than a preset force threshold, triggering the alarm to send out a warning message.
20. The system according to claim 11, further comprising: a display device, wherein the processor is further configured to display the Cartesian force through the display device.
21. A medical robot, comprising: at least two mechanical arms, a mechanical arm comprising a mechanical arm body, the mechanical arm body comprising a plurality of joints; and the system for preventing the collision between mechanical arms according to claim 11.
22. The medical robot according to claim 21, further comprising a physician side, a patient side, and a control end; wherein the physician side, the patient side, and the system for preventing the collision between mechanical arms are respectively in communication with the control end; the mechanical arm is located at the patient side; the physician side comprises a master manipulator, the master manipulator comprises a plurality of joints and a motor driving the joint to move, the master manipulator is configured to control a movement of the mechanical arm; the control end is configured to set a Cartesian force of the master manipulator according to a Cartesian force of the mechanical arm obtained from the system for preventing the collision between the mechanical arms; and the control end is further configured to obtain a torque corresponding to each joint of the master manipulator according to the Cartesian force and a force Jacobian matrix of the master manipulator, and control the motor to output a resistance torque having a same magnitude as but an opposite direction to the torque, to make the Cartesian force of the mechanical arm perceived by an operator.
Description
BRIEF DESCRIPTION OF THE DRAWINGS
[0080] In order to more clearly describe the technical solution in the embodiments or the prior art, the accompanying drawings needing to be used in the description of the embodiment or the prior art will be briefly introduced below. Apparently, the drawings in the following description are merely some embodiments of the present disclosure, and those of ordinary skill in the art can obtain drawings of other embodiments based on these drawings without creative work.
[0081]
[0082]
[0083]
[0084]
[0085]
[0086]
[0087]
[0088]
[0089]
[0090]
[0091]
[0092]
DETAILED DESCRIPTION
[0093] In order to make the objectives, technical solution, and advantages of the present disclosure clearer, the disclosure will be further described in detail with reference to the accompanying drawings and embodiments. It should be appreciated that the specific embodiments described here are only used for explaining the present disclosure, rather than limiting the present disclosure.
[0094] In the present disclosure, for each component, one end adjacent to the patient is a distal end or rear end; and the other end is a proximal end or front end.
[0095] The technical contents of the present disclosure are described in detail below by taking a teleoperation medical robot as an example in combination with the actual situations.
[0096] Teleoperation medical robots generally include components such as a control end, a physician side, and a patient side. Both the physician side and the patient side are in communication with the control end, that is, the physician side controls a relevant medical equipment on the patient side through the control end to perform a corresponding detection or surgery and the like. Generally, more than three mechanical arms are installed on the patient side, and the mechanical arm can include a mechanical arm body (such as a tool arm) and a position sensor. The mechanical arm body can include a plurality of joints and connecting rods connected through the joints; and the position sensor is configured to obtain position information of each joint (for example, a rotation angle of a rotational joint, a movement displacement of a prismatic joint, etc.). At least part of the robotic arms can also include a medical instrument, such as a surgical instrument, or a detection instrument, mounted on the rear end of the above-mentioned robotic arm body; and the physician side can be provided with a master manipulator, that is, an operator can control the detection instrument through the master manipulator in a master-slave manner to acquire a surgical environment, and can control the mechanical arm body and/or surgical instrument through the main manipulator in the master-slave manner to perform collaborative surgical operations, etc.
[0097] It should be noted that, unless otherwise stated in the context, forces and torques of the relevant medical instruments (e.g., medical instruments such as the mechanical arms, surgical instruments mounted on the rear end of the mechanical arm body, detection instruments, etc.) acquired and related to the patient side are virtual physical quantities, i.e., the forces and torques are not actually physically applied between the relevant mechanical arms of the patient side, but are merely parameter values acquired through specific algorithms and/or procedures to describe the urgency of the collision between the mechanical arms. In comparison, the actual physical quantities of the force and torque acquired in relation to the patient side which can represent the urgency of the collision between the mechanical arms may also be virtual physical quantities, i.e., the force and torque here can be perceived by touch from the operator through a specific device (e.g., a master manipulator), or can be perceived by vision or auditory from the operator through a specific device (e.g., a display device or a speaker), which is not limited herein.
[0098]
[0099]
[0100]
[0101] In some embodiments, the memory 12 can store information data such as the information of the relationship between the first relative distance and the first interaction force, information of the size of each mechanical arm, and information of the discrete point. The above information can be stored in the memory 12 in advance, for example, before the surgery, at the time of system initialization or leaving the factory, or the information can be inputted temporarily through an input device during the surgery, and is directly acquired by the processor 11 to perform, or is stored in the memory 12. The information of the size of the mechanical arm can include information of a size of the mechanical arm body, information of a size of the medical instrument (such as a surgical instrument and/or a detection instrument), and the like. Further, when the discrete points are arranged on the mechanical arm, the number and distribution of the discrete points can be determined according to the force collision detection sensitivity, the surgical operation requirements, and the size of each component of the mechanical arm.
[0102] In another optional embodiment, the discrete point arranging unit 14 can arrange discrete points on the mechanical arm body (e.g., tool arm) and/or the medical instrument according to an instruction issued by the processor 11. At the same time, the processor 11 can also output the discrete point information such as a first relative position of each discrete point with respect to a corresponding joint, a discrete point gain coefficient of each discrete point, and the like to the memory 12 for subsequent retrieval by the processor 11 or other devices. In an alternative solution, the memory 12 can also be in communication with the discrete point arranging unit 14, and can also be configured to store discrete point arranging rule information. The discrete point arranging unit 14 can arrange the discrete points on the mechanical arm body (e.g., the tool arm) and/or the medical instrument according to the retrieved discrete point arranging rule information pre-stored in the memory 12. At the same time, the processor 11 can also store the discrete point information such as a first relative position of each discrete point with respect to a corresponding joint, a discrete point gain coefficient of each discrete point, and the like into the memory 12 for subsequent retrieval by the processor 11 or other devices.
[0103] In an optional embodiment, the processor 11 can be further configured to obtain first coordinate information of each discrete point in a global coordinate system according to the second coordinate information of the joints on each mechanical arm in the global coordinate system and the first relative position of the discrete point with respect to the corresponding joint, and obtain the first relative distance between the discrete points according to the first coordinate information of each discrete point, and obtain the first interaction force and a resultant force corresponding to each discrete point based on the corresponding relationship between the first interaction force and the first relative distance between the discrete points. The resultant force can be a resultant force of all first interaction forces corresponding to each discrete point. For any one mechanical arm, the processor 11 can obtain a Cartesian force (i.e., Spatial Force) of the mechanical arm based on the resultant forces corresponding to all discrete points on the mechanical arm and make the Cartesian force perceived by the operator.
[0104] In another optional embodiment of the present disclosure, the discrete point arranging unit 14 in the embodiment of the present disclosure can also be implemented by software, hardware, or a combination of software and hardware, and integrated with the processor 11; or functions of the processor 11 and the discrete point arranging unit 14 can be simultaneously implemented by one processing device.
[0105] It should be noted that the discrete points in the embodiment of the present disclosure can be generated by the system according to a certain rule (for example, the rule may include interval setting information, and then the discrete points can be arranged according to the interval setting information, and the size of the interval can be set according to precision and operation requirements), or can be generated by inputting the rule information by the operator and directly reading the inputted rule information by the processor 11. Each discrete point herein can be configured to represent one volume block on a connecting rod of the mechanical arm, and the discrete point is located at a geometric center of a corresponding volume block. Thus, the greater the number of the discrete points, the greater the number of the volume blocks the discrete points represent, and thus the smaller the volume of each volume block. The relative size of the volume block can be represented by the discrete point gain coefficient g.sub.j. Where, Σg.sub.i=1, i=1, 2, . . . , j, j is the number of all discrete points on a connecting rod of a mechanical arm, and i, j are positive integer, and i<j.
[0106]
[0107] Further, as shown in
[0108] It should be noted that each of the exemplary embodiments in the present disclosure has no particular limitation on the relationship between the first relative distance and the first interaction force. A value range of the first relative distance is (0, +∞), so long as the first interaction force is negatively correlated with the first relative distance within at least one distance interval in the value range (0, +∞). For example, the value range (0, +∞) includes only one distance interval, that is, the first interaction force is negatively correlated with the first relative distance in the distance interval (0, +∞). Alternatively, the entire value range (0, +∞) includes a plurality of distance intervals, the first interaction force and the first relative distance may have different correspondences in different distance intervals. The corresponding relationship between the first relative distance and the first interaction force will be further detailed with reference to
[0109]
[0110] In an optional embodiment, the first interaction force F is a preset maximum interaction force F.sub.max in the range of the first distance interval (0, L.sub.min+R.sub.m+R.sub.n]. In the second distance interval (L.sub.min+R.sub.m+R.sub.n, L.sub.max+R.sub.m+R.sub.n), the relationship between the first interaction force and the first relative distance between the two discrete points is a function which is a continuous function in the second distance interval, and a first derivative of the function is less than zero and a second derivative of the function is greater than zero. In the third distance interval [L.sub.max+R.sub.m+R.sub.n, +∞) the first interaction force F is zero. In other words, in the present embodiment, the first interaction force F.sub.m,n between any two discrete points m, n located on different mechanical arms and the first relative distance L between the two discrete points may satisfy the following relationship:
[0111] Where, F.sub.m,n is the first interaction force of the discrete point in with respect to the discrete point n, and the direction of F.sub.m,n is from the discrete point m to the discrete point n, and a value of F.sub.m,n is equal to that of F.sub.n,m, but a direction of F.sub.m,n is opposite to that of F.sub.n,m; L is a first relative distance between the discrete point m and the discrete point n; R.sub.m is a maximum radius of a section of the mechanical arm at the discrete point in which is on the mechanical arm; and R.sub.n is a maximum radius of a section of the mechanical arm at the discrete point n which is on the mechanical arm; F.sub.max represents the maximum force; f(L) is a continuous function within the second distance interval (L.sub.min+R.sub.m+R.sub.n, L.sub.max+R.sub.m+R.sub.n) , and the first derivative of the function is less than zero and the second derivative is greater than zero. Therefore, as the first relative distance L increases, the value of the first interaction force F.sub.m,n obtained by calculating according to the above-mentioned function decreases at a faster speed.
[0112] In an optional embodiment, the function f(L) can be provided as follows:
[0113] Where, k is a distance-force gain coefficient, L can be a first relative distance, and L′ can be a derivative of L with respect to time, and configured to represent a rate of change of the first relative distance between the two discrete points m, n with respect to time, specifically, which can be obtained by calculating a difference value between a current first relative distance between the two discrete points m and n and a first relative distance between the two discrete points in and n in a previous control period (i.e., collision detection interval time), and dividing the difference value by the control period. The control period can be, for example, 0.5 ms, i.e., a collision detection is performed every 0.5 ms.
[0114]
[0115] In the present embodiment, the operator can preoperatively preset a plurality of distance intervals and a corresponding relationship between the first relative distance and the first interaction force within each distance interval by using the processor 11, and stores in the memory 12 for subsequent retrieval. Apparently, other devices can also be utilized to preset the distance interval and the corresponding relationship between the first relative distance and the first interaction force in each distance interval when the device is initialized or leaves the factory, and pre-store in the memory 12. Further, the processor 11 can acquire an interaction force (i.e., the first interaction force) corresponding to each discrete point and a discrete point on other mechanical arms based on the corresponding relationship between the first relative distance and the first interaction force stored in the memory 12, and obtain the resultant force of all the first interaction forces according to the interaction forces, and then obtain the Cartesian force of each mechanical arm based on the resultant force of all the first interaction forces corresponding to each discrete point. Alternatively, the system for preventing a collision between mechanical arms further includes an input device. The operator can temporarily set a plurality of distance intervals and the corresponding relationship between the first relative distance and the first interaction force within each distance interval through the input device during the surgery. In this case, the processor 11 can also directly acquire the relationship between the first relative distance and the first interaction force.
[0116]
[0117] Specifically, when the first mechanical arm 40 and third mechanical arm 42 are in the configurations shown in
[0118]
[0119] Specifically, the control end can also be configured to scale the Cartesian force of the first mechanical arm 40 to obtain the Cartesian force F.sub.d of the first master manipulator 30 at the physician side. Then, the control end can obtain the resistance torque M.sub.R output by the motor and configured to control the movement of each joint of the first master manipulator 30 according to the force Jacobian matrix of the first master manipulator 30. Similarly, after the torque of each joint of the second master control arm 31 is obtained, the control end can transmit the torque for each joint of the first master manipulator 30 and the second master manipulator 31 to the motor of the master manipulator to output a resistance torque with the same magnitude with but opposite direction to the torque, so that the operator encounters a corresponding resistance during the operation, and accordingly perceives a Cartesian force of each mechanical arm, thereby achieving the purpose of accurately controlling and effectively avoiding the collision between the mechanical arms.
[0120] In another optional embodiment, the system for preventing the collision further includes a display device which is configured to display a Cartesian force corresponding to a distal end of each mechanical arm in an information representation such as text, graphics, images, etc., so that the Cartesian force is perceived by the operator.
[0121] In another optional embodiment, as shown in
[0122] In the above-mentioned embodiments, the system for preventing the collision can be used for detecting the collision between two or more mechanical arms, in which the mechanical arm can include a mounted medical instrument. It will be appreciated by those skilled in the art that the system and method for preventing the collision in the embodiments of the present disclosure can be used for detecting the collision between mechanical arm bodies, collision between the mechanical arm body and the medical instrument, and/or collision between the medical instruments, and the like.
[0123]
[0124] At step S10, a plurality of discrete points are provided on each mechanical arm.
[0125] Specifically, the discrete points are arranged on each mechanical arm according to factors such as force collision detection sensitivity, surgical operation requirements, and the size of each component of the mechanical arm. For example, a plurality of discrete points can be arranged on each mechanical arm body. In order to improve the force collision detection sensitivity, the various discrete points can be uniformly arranged on each connecting rod, or the discrete points can be distributed in a region where collision is relatively easy to occur according to information such as the shape and size of each connecting rod, or each of the discrete points can be uniformly distributed on the mechanical arm. The size information may specifically include various three-dimensional geometric parameters such as lengths, shapes, and volumes of each joint and connecting rod on the mechanical arm.
[0126] In an optional embodiment, the first relative position information of each discrete point with respect to the corresponding joint and the parameter information such as a discrete point gain coefficient of each discrete point can be also continuously obtained.
[0127] It should be noted that when the mechanical arm in the embodiment of the present disclosure includes the medical instrument mounted at the distal end of the mechanical arm body, discrete points can be arranged on the mechanical arm body and/or the medical instrument, that is, the medical instrument can be considered as one connecting rod of the mechanical arm body, so that the anti-collision detection can be performed on the mechanical arm body and the mounted medical instrument as a whole.
[0128] At step 520, first coordinate information of each discrete point in a global coordinate system is acquired.
[0129] Specifically, the second coordinate information of each discrete point in the global coordinate system can be acquired according to joint position information obtained by the position sensor and the preset kinematics model; and the first coordinate information of each discrete point in the global coordinate system is obtained according to the second coordinate information and the first relative position information.
[0130] In an optional embodiment, the plurality of discrete points can be arranged and the first relative position information can be obtained after the second coordinate information is obtained; and then the first coordinate information is obtained according to the second coordinate information and the first relative position information.
[0131] At step S30, a first relative distance between two discrete points located on different mechanical arms is obtained according to the first coordinate information.
[0132] Specifically, the first relative distance between discrete points respectively located on different mechanical arms can be obtained according to the first coordinate information. That is, the distance between any two discrete points respectively located on different mechanical arms is calculated according to the first coordinate information of the two discrete points, to obtain the first relative distance between any two discrete points respectively located on different mechanical arms.
[0133] At step S40, a first interaction force corresponding to each discrete point and each discrete point on the other mechanical arm is acquired according to the first relative distance and a corresponding relationship between the first relative distance and the first interaction force.
[0134] In the exemplary embodiments of the present disclosure, there is no specific limitation on the relationship between the first relative distance and the first interaction force. A value range of the first relative distance is (0, +∞) as long as the first interaction force is negatively correlated with the first relative distance within at least one distance interval in the value range (0, +∞).
[0135] In an optional embodiment, when the value range of the first relative distance includes only one distance interval, i.e., the first interaction force is negatively correlated with the first relative distance in the distance interval (0, +∞).
[0136] In another optional embodiment, the value range of the first relative distance can include a plurality of different distance intervals, and the corresponding relationship between the first relative distance and the first interaction force in each distance interval is at least partially different.
[0137] For example, the value range includes a first distance interval (0, L.sub.min+R.sub.m+R.sub.n], a second distance interval (L.sub.minR.sub.m+R.sub.n, L.sub.max+R.sub.m+R.sub.n), and a third distance interval [L.sub.max+R.sub.m+R.sub.n, +∞). Here, R.sub.m is cross-sectional maximum radius of the mechanical arm (e.g., the first mechanical arm 40) at the discrete point m, in which the discrete point m is located on the mechanical arm; R.sub.n is cross-sectional maximum radius of the mechanical arm (e.g., the second mechanical arm 41) at the discrete point n, in which the discrete point n is located on the mechanical arm; and L.sub.max is a preset collision warning distance; L.sub.min is a preset minimum safe distance. Then, the first interaction force F is a preset maximum interaction force F.sub.max in the first distance interval (0, L.sub.min+R.sub.m+R.sub.n]. The corresponding relationship between the first interaction force and the first relative distance between the two discrete points is a function within the second distance interval (L.sub.min+R.sub.m+R.sub.n, L.sub.max+R.sub.m+R.sub.n); the function is a continuous function in the second distance interval, and the first derivative of the function is less than zero, and the second derivative of the function is greater than zero. The first interaction force F is zero in the third distance interval [L.sub.max+R.sub.m+R.sub.n+∞). The corresponding relationship between the first relative distance and the first interaction force can be represented by the following formula:
[0138] In the formula, F.sub.m,n is the first interaction force of the discrete point in with respect to the discrete point n, with a direction from the discrete point m to the discrete point n, and accordingly, F.sub.n,m is the first interaction force of the discrete point n with respect to the discrete point m with a direction from the discrete point n to the discrete point m, i.e., F.sub.m,n and F.sub.n,m have the same value but opposite directions.
[0139] In another optional embodiment, the function which is continuous in the second distance interval can be:
[0140] Where, k is a distance-force gain coefficient and L is the first relative distance between the discrete points; L′ is a derivative of L with respect to time, and is configured to characterize a rate of change of the first relative distance between the two discrete points with respect to time, which can be obtained by calculating a difference value between a current first relative distance between any two discrete points and a first relative distance between the two discrete points in the previous control period (i.e., collision detection interval time), and dividing the difference value by the control period.
[0141] At step S50, a resultant force of first interaction forces each of which corresponds to each discrete point and each discrete point on the other mechanical arm is calculated.
[0142] Specifically, for any discrete point, a first interaction force corresponding to the discrete point and each of all discrete points located on the other mechanical arm is acquired, and a vector summation is performed on all first interraction forces to obtain a resultant force of all the first interaction forces corresponding to the discrete point.
[0143] At step S60, a Cartesian force of each mechanical arm is acquired and the Cartesian force is perceived by the operator.
[0144] Specifically, a torque of a joint corresponding to each discrete point pair is obtained according to the resultant force of all first interaction forces corresponding to each discrete point and first relative position information of each discrete point with respect to a corresponding joint; and a resultant torque corresponding to each joint is obtained by combining the discrete point gain coefficients g.sub.i; and then, the Cartesian force of each mechanical arm is obtained according to the resultant torque corresponding to each joint and a force Jacobian matrix of the mechanical arm.
[0145] In the present embodiment, there is no specifically limit to the method by which the Cartesian force of each mechanical arm is perceived by the operator. In a specific embodiment, the Cartesian force of the master manipulator 31 controlling the movement of the mechanical arm can be obtained according to the acquired Cartesian force of the mechanical arm, and then the torque of each joint of the master manipulator 31 can be obtained by means of such as the force Jacobian matrix of the mechanical arm; and the motor is subsequently controlled to output a resistance torque with the same magnitude as but opposite direction to the torque of each joint of the master manipulator 31, so that the operator's hand can perceive the Cartesian force of the mechanical arm.
[0146] In another specific embodiment, the Cartesian force can also be displayed through a display device in an information representation such as text, graphics, images, etc., so that the operator can perceive the Cartesian force through eyes.
[0147] In another optional embodiment, when the distance between any two discrete points respectively located on different mechanical arms is less than a preset distance threshold, or the obtained Cartesian force is greater than a predetermined alarm force, a warning message such as sound, light, electricity, vibration, or the like is triggered. For example, the distance threshold can be set to L.sub.min+R.sub.m+R.sub.n. For another example, the distance threshold can include a first distance threshold L.sub.min+R.sub.m+R.sub.n and a second distance threshold L.sub.max+R.sub.m+R.sub.n; and when the first relative distance L between the two discrete points is less than the first distance threshold L.sub.min+R.sub.m+R.sub.n, the alarm sends out first alarm information (e.g., sending out a red light signal); when the first relative distance L between the two discrete points is greater than or equal to the first distance threshold L.sub.min+R.sub.m+R.sub.n and less than the second distance threshold L.sub.max+R.sub.m+R.sub.n, the alarm sends out second alarm information (e.g., sending out a yellow light signal); when the first relative distance L between the two discrete points is greater than the second distance threshold L.sub.max+R.sub.mR.sub.n, the alarm is dormant or sends out normal operation information (e.g., sending out a green light signal).
[0148] It should be appreciated that although the steps in the flow chart of
[0149] Persons of ordinary skill in the art understand that all or part of the processes in the methods of the above-mentioned embodiments may be implemented by a computer program instructing a relevant hardware. The computer program may be stored in a non-transitory computer-readable storage medium. When the computer program is executed, flows in the embodiments of the method can be included. Any reference to the memory, storage, database, or other media used in various embodiments provided herein may include a non-transitory and/or transitory memory. The non-transitory memory may include a read only memory (ROM), a programmable ROM (PROM), an electrically programmable ROM (EPROM), an electrically erasable programmable ROM (EEPROM), or a flash memory. The transitory memory may include a random access memory (RAM) or an external cache memory. By way of illustration and not limitation, RAM is available in a variety of forms such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), Synchlink DRAM (SLDRMvi), Memory Bus (Rambus) Direct RAM (RDRAM), Direct Memory Bus Dynamic RAM (DRDRAM), Memory Bus Dynamic RAM (RDRAM) etc.
[0150] The above-mentioned various technical features involved in the respective embodiments can be combined arbitrarily, for brevity, not all possible combinations of the technical features in the above-mentioned embodiments are described. However, as long as there is no conflict in the combinations of the technical features, the combinations of the technical features shall be considered as the scope of the disclosure.
[0151] The above-mentioned embodiments are merely some exemplary embodiments of the present disclosure. The descriptions are more specific and detailed, and are not intended to limit the protection scope of the present disclosure. It should be pointed out that those of ordinary skill in the art can make several modifications and improvements without departing from the concept of the disclosure, and these all fall within the scope of protection of the disclosure. Therefore, the protection scope of the present disclosure shall be subject to the appended claims.