CN114376726B - Path planning method and related device for transcranial magnetic stimulation navigation process - Google Patents

Path planning method and related device for transcranial magnetic stimulation navigation process Download PDF

Info

Publication number
CN114376726B
CN114376726B CN202210118146.0A CN202210118146A CN114376726B CN 114376726 B CN114376726 B CN 114376726B CN 202210118146 A CN202210118146 A CN 202210118146A CN 114376726 B CN114376726 B CN 114376726B
Authority
CN
China
Prior art keywords
mechanical arm
target
magnetic stimulation
pose
coordinates
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202210118146.0A
Other languages
Chinese (zh)
Other versions
CN114376726A (en
Inventor
秦伟
曹霞霞
孙金铂
龙戈农
崔亚朋
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Xi'an Keyue Medical Technology Co ltd
Original Assignee
Xi'an Keyue Medical Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Xi'an Keyue Medical Technology Co ltd filed Critical Xi'an Keyue Medical Technology Co ltd
Priority to CN202210118146.0A priority Critical patent/CN114376726B/en
Publication of CN114376726A publication Critical patent/CN114376726A/en
Application granted granted Critical
Publication of CN114376726B publication Critical patent/CN114376726B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
    • A61B34/10—Computer-aided planning, simulation or modelling of surgical operations
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
    • A61B34/20—Surgical navigation systems; Devices for tracking or guiding surgical instruments, e.g. for frameless stereotaxis
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
    • A61B34/70—Manipulators specially adapted for use in surgery
    • A61B34/73—Manipulators for magnetic surgery
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61N—ELECTROTHERAPY; MAGNETOTHERAPY; RADIATION THERAPY; ULTRASOUND THERAPY
    • A61N2/00—Magnetotherapy
    • A61N2/004—Magnetotherapy specially adapted for a specific therapy
    • A61N2/006—Magnetotherapy specially adapted for a specific therapy for magnetic stimulation of nerve tissue
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61N—ELECTROTHERAPY; MAGNETOTHERAPY; RADIATION THERAPY; ULTRASOUND THERAPY
    • A61N2/00—Magnetotherapy
    • A61N2/02—Magnetotherapy using magnetic fields produced by coils, including single turn loops or electromagnets
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
    • A61B34/10—Computer-aided planning, simulation or modelling of surgical operations
    • A61B2034/101—Computer-aided simulation of surgical operations
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
    • A61B34/10—Computer-aided planning, simulation or modelling of surgical operations
    • A61B2034/107—Visualisation of planned trajectories or target regions
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
    • A61B34/70—Manipulators specially adapted for use in surgery
    • A61B34/73—Manipulators for magnetic surgery
    • A61B2034/731—Arrangement of the coils or magnets

Abstract

The application is suitable for the technical field of medical treatment, provides a path planning method and a related device for a transcranial magnetic stimulation navigation process, and enriches the technical scheme of path planning for the transcranial magnetic stimulation navigation process. The method mainly comprises the following steps: determining the initial position point coordinates of a tool center point of the mechanical arm and the target magnetic stimulation point coordinates of the head of the patient; setting a geometric envelope box for wrapping all magnetic stimulation point coordinates of the head of the patient on the head of the patient; projecting the target magnetic stimulation point coordinates to the surface of the geometric envelope box to obtain target mapping point coordinates of the target magnetic stimulation point coordinates; calculating a first section path of the tool center point of the mechanical arm moving from the initial position point coordinate to the target mapping point coordinate by using an artificial potential field method; calculating a second section path of the tool center point of the mechanical arm from the target mapping point coordinate to the target magnetic stimulation point coordinate by using a mechanical arm inverse kinematics solving algorithm; and connecting the first section of path and the second section of path in sequence to obtain a planned path of the mechanical arm.

Description

Path planning method and related device for transcranial magnetic stimulation navigation process
Technical Field
The application belongs to the technical field of medical treatment, and particularly relates to a path planning method and a related device for a transcranial magnetic stimulation navigation process.
Background
Transcranial magnetic stimulation (transcranial magnetic stimulation, TMS) technology is a neural regulation technology without wound and clear side effects, and the basic principle is as follows: the pulse magnetic field is used to act on central nervous system (mainly cerebral cortex), and the induced current generated by the pulse magnetic field can change the membrane potential of cortical nerve cells, thereby affecting the metabolic activity and neural activity in brain. The stimulation modes of the current transcranial magnetic stimulation technology mainly comprise: single pulse, double pulse and repetitive pulse. Single and double pulse stimulation modes are commonly used for conventional electrophysiological examinations. The repetitive pulse pattern can be used for treating dyskinesia, mental diseases, pathologic pain, epilepsy, addiction, functional recovery after nervous system injury, etc.
In the using process of transcranial magnetic stimulation technology, a magnetic stimulation actuating mechanism for treating the head of a patient is generally a mechanical arm, and the free end of the mechanical arm carries a magnetic stimulation coil to reach the head of the patient for treatment according to the guidance of a planned path. However, there are few solutions in the prior art for robotic arm path planning corresponding to transcranial magnetic stimulation navigation procedures.
Disclosure of Invention
The invention aims to provide a path planning method and a related device for a transcranial magnetic stimulation navigation process, and enriches the technical scheme of path planning in the current transcranial magnetic stimulation navigation process.
In a first aspect, the present application provides a path planning method for a transcranial magnetic stimulation navigation process, applied to a mechanical arm, including:
determining initial position point coordinates of a tool center point of the mechanical arm and target magnetic stimulation point coordinates of the head of a patient;
setting a geometric envelope box for the head of the patient, wherein the geometric envelope box wraps all magnetic stimulation point coordinates of the head of the patient;
projecting the target magnetic stimulation point coordinates to the surface of the geometric envelope box to obtain target mapping point coordinates of the target magnetic stimulation point coordinates on the surface of the geometric envelope box;
calculating a first path of the tool center point of the mechanical arm moving from the initial position point coordinate to the target mapping point coordinate by using an artificial potential field method;
calculating a second path of the tool center point of the mechanical arm moving from the target mapping point coordinate to the target magnetic stimulation point coordinate by using a mechanical arm inverse kinematics solving algorithm;
And connecting the first section of path and the second section of path in sequence to obtain a planned path of the mechanical arm.
Optionally, the disposing a geometry envelope box on the patient's head includes:
and performing cubic surrounding calculation on the head of the patient by using a surrounding box algorithm to obtain a cubic enveloping box for the head of the patient.
Optionally, the projecting the target magnetic stimulation point coordinates onto the surface of the geometric envelope box, and obtaining the target mapping point coordinates of the target magnetic stimulation point coordinates on the surface of the geometric envelope box includes:
calculating the center point coordinates of the cube envelope box;
taking the center point coordinates of the cubic envelope box as the head center point coordinates of the head of the patient;
connecting the head center point coordinate with the target magnetic stimulation point coordinate to obtain a straight line L, wherein the straight line L intersects the cube envelope box at two intersection points;
and taking one of the two intersection points, which is close to the target magnetic stimulation point coordinate, as the target mapping point coordinate of the target magnetic stimulation point coordinate on the surface of the geometric envelope box.
Optionally, the calculating, by using the artificial potential field method, a first path of movement of the tool center point of the mechanical arm from the initial position point coordinate to the target mapping point coordinate includes:
Determining the current mechanical arm pose of the mechanical arm;
calculating the predicted mechanical arm pose corresponding to the k joints of the mechanical arm when the k joints rotate by-lambda degrees, 0 degrees and lambda degrees respectively, wherein the predicted mechanical arm pose corresponds to 3 k A combination of predicted joint angles, wherein lambda is any value from 0 to 360;
calculating the sum potential energy of the tool center points of each predicted mechanical arm pose to obtain 3 k Seed and potential energy;
from said 3 k Selection from species and potential energyAnd predicting the pose of the mechanical arm with the minimum potential energy, wherein the pose is used as the pose of the target mechanical arm for the next movement of the mechanical arm;
judging whether the coordinates corresponding to the tool center point of the pose of the target mechanical arm are equal to the coordinates of the target mapping point;
if the coordinates corresponding to the tool center points of the target mechanical arm pose are equal to the coordinates of the target mapping points, connecting the initial position point coordinates with the coordinates corresponding to the tool center points of all the target mechanical arm poses to obtain the first section of path;
and if the coordinates corresponding to the tool center point of the target mechanical arm pose are not equal to the coordinates of the target mapping point, regarding the target mechanical arm pose as the current mechanical arm pose, and triggering and executing the step of calculating the predicted mechanical arm pose corresponding to the k joints of the mechanical arm when the k joints rotate by-lambda degrees, 0 degrees and lambda degrees respectively.
Optionally, before calculating the sum potential of the tool center points of each predicted robot pose, the method further comprises:
performing collision detection on the predicted mechanical arm pose, and determining m joint angle combinations in which the predicted mechanical arm pose cannot collide, wherein m is more than 0 and less than or equal to 3 k Is a positive integer of (2);
the sum potential energy of the tool center points of each predicted mechanical arm pose is calculated to obtain 3 k The sum potential energy includes:
and calculating the sum potential energy of the center points of the m tools for predicting the pose of the mechanical arm to obtain m kinds of sum potential energy.
Optionally, the performing collision detection on the predicted pose of the mechanical arm, and determining m joint angle combinations where the predicted pose of the mechanical arm cannot collide includes:
judging whether the model data of the mechanical arm of each predicted mechanical arm pose has intersection with the geometric envelope box or not;
if the intersection exists, eliminating a collision joint angle combination corresponding to the predicted mechanical arm pose of the intersection of the geometric envelope box;
if the intersection does not exist, summarizing all joint angle combinations corresponding to the predicted mechanical arm pose without the intersection, and obtaining m joint angle combinations in which the mechanical arm pose is not collided.
Optionally, the model data of the mechanical arm includes coil model data of a magnetic stimulation coil, the magnetic stimulation coil is installed at the tail end of the mechanical arm in an adapting way, a tool center point of the mechanical arm is located on the free end surface of the magnetic stimulation coil, and a coordinate value of the tool center point of the mechanical arm is equal to a coordinate value corresponding to a coordinate of the free end center point of the mechanical arm plus a thickness value of the magnetic stimulation coil.
Optionally, the calculating the sum potential energy of the tool center points of each predicted mechanical arm pose includes:
establishing a repulsive field of the geometric envelope box and a gravitational field of the target mapping point;
calculating repulsive potential energy of a tool center point of each predicted mechanical arm pose in the repulsive force field;
calculating gravitational potential energy received by a tool center point of each predicted mechanical arm pose in the gravitational field;
and comprehensively calculating the sum potential energy of the repulsive potential energy and the gravitational potential energy received by the tool center point of each predicted mechanical arm pose.
In a second aspect, the present application provides a path planning system for a transcranial magnetic stimulation navigation process, for use with a robotic arm, comprising:
the determining unit is used for determining initial position point coordinates of a tool center point of the mechanical arm and target magnetic stimulation point coordinates of the head of the patient;
A setting unit, configured to set a geometric envelope box for the patient's head, where the geometric envelope box wraps all magnetic stimulation point coordinates of the patient's head;
the mapping unit is used for projecting the target magnetic stimulation point coordinates to the surface of the geometric envelope box to obtain target mapping point coordinates of the target magnetic stimulation point coordinates on the surface of the geometric envelope box;
the calculation unit is used for calculating a first section path of the tool center point of the mechanical arm moving from the initial position point coordinate to the target mapping point coordinate by using an artificial potential field method;
the calculation unit is also used for calculating a second section path of the tool center point of the mechanical arm moving from the target mapping point coordinate to the target magnetic stimulation point coordinate by utilizing a mechanical arm inverse kinematics solving algorithm;
the connecting unit is used for connecting the first section path and the second section path in sequence to obtain a planned path of the mechanical arm.
Optionally, when the setting unit sets a geometric envelope box for the head of the patient, the setting unit specifically includes:
and performing cubic surrounding calculation on the head of the patient by using a surrounding box algorithm to obtain a cubic enveloping box for the head of the patient.
Optionally, the mapping unit projects the target magnetic stimulation point coordinates to the surface of the geometric envelope box, so as to obtain the target mapping point coordinates of the target magnetic stimulation point coordinates on the surface of the geometric envelope box, which is specifically used for:
calculating the center point coordinates of the cube envelope box;
taking the center point coordinates of the cubic envelope box as the head center point coordinates of the head of the patient;
connecting the head center point coordinate with the target magnetic stimulation point coordinate to obtain a straight line L, wherein the straight line L intersects the cube envelope box at two intersection points;
and taking one of the two intersection points, which is close to the target magnetic stimulation point coordinate, as the target mapping point coordinate of the target magnetic stimulation point coordinate on the surface of the geometric envelope box.
Optionally, the calculating unit calculates a first path of the tool center point of the mechanical arm from the initial position point coordinate to the target mapping point coordinate by using an artificial potential field method, and specifically is configured to:
determining the current mechanical arm pose of the mechanical arm;
calculation stationThe k joints of the mechanical arm rotate respectively by-lambda degrees, 0 degrees and lambda degrees to correspondingly predict the pose of the mechanical arm, and the predicted pose of the mechanical arm corresponds to 3 k A combination of predicted joint angles, wherein lambda is any value from 0 to 360;
calculating the sum potential energy of the tool center points of each predicted mechanical arm pose to obtain 3 k Seed and potential energy;
from said 3 k The predicted mechanical arm pose with the minimum seed and potential energy is selected as the target mechanical arm pose of the mechanical arm moving next step;
judging whether the coordinates corresponding to the tool center point of the pose of the target mechanical arm are equal to the coordinates of the target mapping point;
if the coordinates corresponding to the tool center points of the target mechanical arm pose are equal to the coordinates of the target mapping points, connecting the initial position point coordinates with the coordinates corresponding to the tool center points of all the target mechanical arm poses to obtain the first section of path;
and if the coordinates corresponding to the tool center point of the target mechanical arm pose are not equal to the coordinates of the target mapping point, regarding the target mechanical arm pose as the current mechanical arm pose, and triggering and executing the step of calculating the predicted mechanical arm pose corresponding to the k joints of the mechanical arm when the k joints rotate by-lambda degrees, 0 degrees and lambda degrees respectively.
Optionally, the system further comprises:
the detection unit is used for carrying out collision detection on the predicted mechanical arm pose and determining m joint angle combinations in which the predicted mechanical arm pose cannot collide, wherein m is more than 0 and less than or equal to 3 k Is a positive integer of (2);
the calculation unit calculates the sum potential energy of the tool center points of each predicted mechanical arm pose to obtain 3 k The method is particularly used for the preparation of the composite material:
and calculating the sum potential energy of the center points of the m tools for predicting the pose of the mechanical arm to obtain m kinds of sum potential energy.
Optionally, the detecting unit performs collision detection on the predicted pose of the mechanical arm, and is specifically configured to:
judging whether the model data of the mechanical arm of each predicted mechanical arm pose has intersection with the geometric envelope box or not;
if the intersection exists, eliminating a collision joint angle combination corresponding to the predicted mechanical arm pose of the intersection of the geometric envelope box;
if the intersection does not exist, summarizing all joint angle combinations corresponding to the predicted mechanical arm pose without the intersection, and obtaining m joint angle combinations in which the mechanical arm pose is not collided.
Optionally, the model data of the mechanical arm includes coil model data of a magnetic stimulation coil, the magnetic stimulation coil is installed at the tail end of the mechanical arm in an adapting way, a tool center point of the mechanical arm is located on the free end surface of the magnetic stimulation coil, and a coordinate value of the tool center point of the mechanical arm is equal to a coordinate value corresponding to a coordinate of the free end center point of the mechanical arm plus a thickness value of the magnetic stimulation coil.
Optionally, when the computing unit computes the sum potential energy of the tool center points of each predicted mechanical arm pose, the computing unit is specifically configured to:
establishing a repulsive field of the geometric envelope box and a gravitational field of the target mapping point;
calculating repulsive potential energy of a tool center point of each predicted mechanical arm pose in the repulsive force field;
calculating gravitational potential energy received by a tool center point of each predicted mechanical arm pose in the gravitational field;
and comprehensively calculating the sum potential energy of the repulsive potential energy and the gravitational potential energy received by the tool center point of each predicted mechanical arm pose.
In a third aspect, the present application provides a computer device comprising:
processor, memory, bus, input/output interface, network interface;
the processor is connected with the memory, the input/output interface and the network interface through buses;
the memory stores a program;
the processor, when executing the program stored in the memory, implements the path planning method of the transcranial magnetic stimulation navigation process according to the first aspect.
In a fourth aspect, the present application provides a computer-readable storage medium having instructions stored therein, which when executed on a computer, cause the computer to perform a path planning method of a transcranial magnetic stimulation navigation procedure as described in the first aspect.
In a fifth aspect, the present application provides a computer program product which, when executed on a computer, causes the computer to perform a path planning method of a transcranial magnetic stimulation navigation procedure as described in the first aspect.
The above technical solution can be seen that the embodiment of the application has the following advantages:
the method comprises the steps of determining initial position point coordinates of a tool center point of a mechanical arm and target magnetic stimulation point coordinates of the head of a patient; then, a geometric body enveloping box is arranged on the head of the patient, wherein the geometric body enveloping box wraps all magnetic stimulation point coordinates of the head of the patient; projecting the target magnetic stimulation point coordinates to the surface of the geometric envelope box to obtain target mapping point coordinates of the target magnetic stimulation point coordinates on the surface of the geometric envelope box; calculating a first section path of the tool center point of the mechanical arm moving from the initial position point coordinate to the target mapping point coordinate by using an artificial potential field method; calculating a second section path of the tool center point of the mechanical arm from the target mapping point coordinate to the target laser point coordinate by utilizing a mechanical arm inverse kinematics solving algorithm; and connecting the first section of path with the second section of path in sequence to obtain a planned path of the mechanical arm. Therefore, when the magnetic stimulation center point of the magnetic stimulation coil is positioned at the tool center point of the mechanical arm, the mechanical arm can command the tool center point of the mechanical arm to move from the initial position point coordinate to the target magnetic stimulation point coordinate according to the planning path, so that the path planning navigation of the mechanical arm in the transcranial magnetic stimulation navigation process is realized.
Drawings
FIG. 1 is a flow diagram of one embodiment of a path planning method for transcranial magnetic stimulation navigation according to the present application;
FIG. 2 is a flow chart of another embodiment of a path planning method of the transcranial magnetic stimulation navigation procedure of the present application;
FIG. 3 is a flow chart of another embodiment of a path planning method of the transcranial magnetic stimulation navigation procedure of the present application;
FIG. 4 is a schematic diagram of one embodiment of a path planning system for transcranial magnetic stimulation navigation according to the present application;
FIG. 5 is a schematic diagram illustrating the structure of one embodiment of a computer device of the present application;
FIG. 6 is an effect diagram of one embodiment of a patient head model setup cube envelope box of the present application;
FIG. 7 is a schematic structural view of an embodiment of a six-axis mechanical arm of the present application;
FIG. 8 is a graph showing the effect of one embodiment of the coordinates of the target magnetic stimulation point where the tool center point of the six-axis mechanical arm of the present application abuts the head of the patient.
Detailed Description
In order to make the objects, technical solutions and advantages of the present application more apparent, the present application will be further described in detail with reference to the accompanying drawings and examples. It should be understood that the specific embodiments described herein are for purposes of illustration only and are not intended to limit the present application.
It can be understood that, the path planning method in the transcranial magnetic stimulation navigation process of the present application is established on the premise that the spatial coordinate system where the mechanical arm is located has already known the three-dimensional coordinate set of the patient head model, and the mapping and registration of the patient head model in the spatial coordinate system where the mechanical arm is located have already been mature prior art schemes, which are not described herein. For example, a head model of a patient is created in advance, then the head of the patient is visually positioned through a binocular vision camera, the position and the posture of the head of the patient in a binocular vision camera coordinate system are known, then the position and the posture of the head model of the patient in the binocular vision camera coordinate system can be mapped to the space coordinate system where the mechanical arm is located according to the coordinate system conversion relation between the binocular vision camera coordinate system and the space coordinate system where the mechanical arm is located, and the space coordinate system where the mechanical arm is located can be made to know the position and the posture of the head of the patient.
Referring to fig. 1, an embodiment of a path planning method for a transcranial magnetic stimulation navigation process is applied to a mechanical arm, and includes:
101. and determining the initial position point coordinates of the tool center point of the mechanical arm and the target magnetic stimulation point coordinates of the head of the patient.
The initial position point coordinate of the tool center point of the mechanical arm needs to be determined in order to obtain the starting point of the path planning technical scheme in the transcranial magnetic stimulation navigation process, for example, the initial position point coordinate can be a coordinate point of the tool center point of the mechanical arm in a space coordinate system of the mechanical arm when the mechanical arm is started, and the tool center point of the mechanical arm generally automatically returns to the known original point of the mechanical arm when the mechanical arm is started. The tool center point of the robotic arm generally refers to the center point (Tool Central Point, TCP) of the tool (e.g., magnetic stimulation coil) mounted at the end of the robotic arm.
The step further needs to determine the target magnetic stimulation point coordinates of the head of the patient so as to obtain the end point of the path planning technical scheme of the transcranial magnetic stimulation navigation process, for example, the target magnetic stimulation point coordinates can be coordinate points selected by an operator on a head model of the patient, and the target magnetic stimulation point coordinates correspond to corresponding magnetic stimulation treatment points on the head of the patient lying in the moving range of the mechanical arm in reality.
102. A geometric envelope box is arranged on the head of the patient, and the geometric envelope box wraps all magnetic stimulation point coordinates of the head of the patient.
On the premise that the space coordinate system of the mechanical arm is known to be a three-dimensional coordinate set of a head model of a patient, the geometric envelope box can be arranged on the head (model) of the patient, so that the geometric envelope box wraps all magnetic stimulation point coordinates of the head of the patient. Specifically, a bounding box algorithm is used to perform a cubic bounding calculation on the patient's head, resulting in a cubic bounding box for the patient's head, such as shown in fig. 6.
103. And projecting the target magnetic stimulation point coordinates to the surface of the geometric envelope box to obtain target mapping point coordinates of the target magnetic stimulation point coordinates on the surface of the geometric envelope box.
Since the geometric envelope box is set for the patient's head in step 102, which wraps all the magnetic stimulation point coordinates of the patient's head, then the present step may project the target magnetic stimulation point coordinates onto the surface of the geometric envelope box, to obtain the target mapping point coordinates of the target magnetic stimulation point coordinates on the surface of the geometric envelope box.
For example, referring to fig. 6, assume that the target magnetic stimulus point coordinates G (x g ，y g ，z g ) At the forehead of the patient's head model, the coordinates O (x o ，y o ，z o ) The center point coordinates O (x o ，y o ，z o ) As head center point coordinates of the patient's head; connect the head center point coordinates and the target magnetic stimulation point coordinates G (x g ，y g ，z g ) Obtaining a straight line L, wherein the straight line L intersects the cube envelope box at two intersection points, and then selecting one point J (x) close to the coordinates of the target magnetic stimulation point from the two intersection points j ，y j ，z j ) Target mapping point coordinates J (x) as target magnetic stimulation point coordinates at the surface of the geometric envelope box j ，y j ，z j )。
104. And calculating a first section path of the tool center point of the mechanical arm from the initial position point coordinate to the target mapping point coordinate by using an artificial potential field method.
The present step uses an artificial potential field method to implement a process of calculating a first path of a tool center point of the mechanical arm from an initial position point coordinate to a target mapping point coordinate, and specifically refer to the embodiment of fig. 2. In the actual application process, the specific parameters of the artificial potential field method may be adjusted according to the actual situation, and the specific calculation process of the first path from the initial position point coordinate to the target mapping point coordinate of the tool center point of the mechanical arm calculated by using the artificial potential field method is not limited herein.
105. And calculating a second path of the tool center point of the mechanical arm from the target mapping point coordinate to the target magnetic stimulation point coordinate by using a mechanical arm inverse kinematics solving algorithm.
The mechanical arm inverse kinematics solving algorithm is used for calculating the resolvability of the mechanical arm from an initial point to a target point, namely, the conditions of no solution, multiple solutions and the like are considered. That is, for a multi-axis robot, if the target point is within the reach of the arm, there may be multiple solutions for the joint angle combinations of the individual joint motions as the arm moves from the initial point to the target point; if the target point is out of the reachable range of the mechanical arm, the mechanical arm cannot reach the target point out of the reachable range by changing the joint angle combination of each joint movement of the mechanical arm, and no solution exists at the moment.
For example, referring to fig. 7, for a typical six-axis mechanical arm, the first three joints (base joint 710, shoulder joint 720, elbow joint 730) are larger, and the second three joints (wrist 1 joint 740, wrist 2 joint 750, wrist 3 joint 760) are smaller. In general, when defining the distance between the mechanical arm and the space coordinate system, different joints should be given different weights, for example, the first three joints are set with large weights, and the second three joints are set with small weights. Then the smaller joint is preferentially moved over the larger joint when selecting the solution. When an obstacle exists, if the motion path of the "nearest" solution collides with the motion path, another solution with a longer motion distance is selected (the "solution"). Therefore, when considering problems such as collision, path planning and the like, all possible solutions need to be calculated, and then an optimal solution is obtained according to the calculation of the preset weight.
Assuming that the tool center point of the mechanical arm in this step moves from the target mapping point coordinate to the target magnetic stimulation point coordinate, 8 groups of solutions are obtained after the mechanical arm inverse kinematics solution is performed in the 6-axis mechanical arm, the mechanical arm inverse kinematics solution algorithm is a relatively mature prior art, the calculation process is not described herein, please refer to the following table 1 in combination:
TABLE 1
The units of the numerical positions in table 1 are radians (-2pi indicates one revolution in reverse, 2pi indicates one revolution in forward), the positive number indicates the radian of the corresponding value of the joint to be rotated in the preset positive direction, and the negative number indicates the radian of the corresponding value of the joint to be rotated in the preset reverse direction (opposite to the preset positive direction). As shown in the table, for the 6-axis robot, 8 groups of solutions exist for moving the tool center point of the mechanical arm from the target mapping point coordinate to the target magnetic stimulation point coordinate, that is, the tool center point of the mechanical arm can be moved from the target mapping point coordinate to the target magnetic stimulation point by correspondingly executing the rotation of 8 different joint angle combinations; this step may then assign different weights to different joints, for example: p=base joint×a+shoulder joint×b+elbow joint×c+wrist 1 joint×d+wrist 2 joint×e+wrist 3 joint×f, where P represents the equivalent radian of a solution of a certain group after conversion, a represents the preset weight of the base joint, b represents the preset weight of the shoulder joint, c represents the preset weight of the elbow joint, d represents the preset weight of the wrist 1 joint, e represents the preset weight of the wrist 2 joint, f represents the preset weight of the wrist 3 joint; the absolute values of the joints of the mechanical arm are all calculated in the formula, and preferably, a > b > c > d > e > f are taken, so that when a plurality of solutions exist, the joint angle combination with smaller equivalent radian after conversion is preferentially considered in selecting the solutions rather than the joint angle combination with larger equivalent radian after conversion, and a group of most economical and energy-saving target solutions can be screened out, and the group of target solutions enables a path from the coordinates of the target mapping points to the coordinates of the target magnetic stimulation points of the tool center point (see the center point of the magnetic stimulation coil 770 in fig. 7) of the mechanical arm to be the second section of path.
106. And connecting the first section of path and the second section of path in sequence to obtain a planned path of the mechanical arm.
In step 104, a first path from the initial position point coordinate to the target mapping point coordinate of the tool center point of the mechanical arm is obtained, and in step 105, a second path from the target mapping point coordinate to the target magnetic stimulation point coordinate is obtained, then the first path and the second path may be connected in sequence, so as to obtain a planned path of the mechanical arm.
Referring to fig. 8 in combination, when the magnetic stimulation center point of the magnetic stimulation coil is located at the tool center point of the mechanical arm, the mechanical arm can command the tool center point of the mechanical arm to move from the initial position point coordinate to the target magnetic stimulation point coordinate according to the planned path, so as to implement the planned path execution of the mechanical arm in the transcranial magnetic stimulation navigation process.
Referring to fig. 2, in step 104 of the embodiment of fig. 1, the process of calculating the first path of the tool center point of the mechanical arm moving from the initial position point coordinate to the target mapping point coordinate by using the artificial potential field method may specifically include:
201. and determining the current mechanical arm pose of the mechanical arm.
Specifically, the current mechanical arm pose of the mechanical arm can be determined by acquiring the current angle position of each joint of the mechanical arm at the current time, the coordinates of the tool center point, the mechanical arm model data and the like.
For example, assuming that the robot arm is a 6-axis robot arm, this step acquires the current angle position a (θ 1 ，θ 2 ，θ 3 ，θ 4 ，θ 5 ，θ 6 ) And tool center point coordinates for a 6-axis robotic arm, wherein θ 1 、θ 2 、θ 3 、θ 4 、θ 5 、θ 6 Respectively representing the joint angles of the base joint, the shoulder joint, the elbow joint, the wrist 1, the wrist 2 and the wrist 3, and also obtaining model data of the 6-axis mechanical arm, so as to obtain the space size, the space position and the space posture occupied by the whole 6-axis mechanical arm; and combining the current angle positions of all joints of the mechanical arm, the coordinates of the tool center point and the model data of the mechanical arm to obtain the current mechanical arm gesture of the corresponding spatial position of the mechanical arm. The model data of the mechanical arm comprises coil model data of a magnetic stimulation coil, the magnetic stimulation coil is adaptively arranged at the tail end of the mechanical arm, the tool center point of the mechanical arm is positioned on the free end surface of the magnetic stimulation coil, and the coordinate value of the tool center point of the mechanical arm is equal to the coordinate value of the free end center point of the mechanical armCoordinate values corresponding to the thickness values of the magnetic stimulation coils are marked. In another embodiment, the model data of the mechanical arm in this step can be simply equivalent to a specific connecting rod structure between two adjacent joints, each connecting rod is a cylinder with a preset radius, and the commonly used 8-shaped magnetic stimulation coil is equivalent to two cylinders.
202. And calculating the predicted mechanical arm pose corresponding to the k joints of the mechanical arm when the k joints rotate by-lambda degrees, 0 degrees and lambda degrees respectively, wherein the predicted mechanical arm pose corresponds to a plurality of predicted joint angle combinations, and lambda is any numerical value from 0 to 360.
Under the condition that the current mechanical arm pose corresponding to the mechanical arm is obtained in step 201, in this step, the current mechanical arm pose can be taken as a basis, and by calculating the predicted mechanical arm poses corresponding to the cases that all joints of the predicted mechanical arm are respectively increased by-lambda degrees, 0 degrees and lambda degrees, lambda can be any value from 0 to 360, the predicted mechanical arm pose of the mechanical arm at the next moving position can be obtained in this step, namely, the joint angle combination of the various mechanical arm poses of the mechanical arm at the next moving position in the reachable range can be obtained. For example, λ is preferably 5, considering that only k joints in the arm rotate, the predicted arm pose of the arm at the next moving position corresponds to 3 k The combination of the angles of the joints is predicted, namely each joint in k joints in the mechanical arm rotates for 5 degrees (or does not rotate) at most each time, and the combination of the angles of the joints is 3 k The mechanical arm takes joint rotation precision as a moving step length, the smaller the numerical value of lambda is, the higher the precision of each movement of the mechanical arm is, but the longer the time required to be planned and calculated is, in practical application, the choice can be made according to practical requirements, and in the step, each joint of the mechanical arm rotates once at most in a predicted mechanical arm pose.
203. And performing collision detection on all the predicted mechanical arm pose, and determining m joint angle combinations in which the predicted mechanical arm pose cannot collide.
The present step may further perform collision detection on each predicted robot pose in step 202, so as to determine m joint angle combinations that do not collide with the predicted robot pose, whichM is greater than 0 and less than or equal to 3 k The mechanical arm is prevented from selecting the joint angle combination which is likely to collide in the subsequent steps to be executed according to the predicted mechanical arm pose, the head of a patient is prevented from being accidentally injured by the mechanical arm, and the joint angle combination which is likely to collide is eliminated, so that the workload of the subsequent step pairs and potential energy calculation can be reduced. The method for detecting the collision of the predicted pose of the mechanical arm is various, and can be selected according to actual conditions in practical application, and the method for detecting the collision of the predicted pose of the mechanical arm is not limited. In particular, please refer to the embodiment of fig. 3 for detecting the collision of the predicted robot pose.
204. And calculating the sum potential energy of the center points of the m tools for predicting the pose of the mechanical arm to obtain m kinds of sum potential energy.
Specifically, a geometric envelope box except the target mapping point is set as an obstacle, and the tool center point of the mechanical arm needs to avoid contact with the obstacle, so that a repulsive force field of the geometric envelope box needs to be established in the step, and according to a repulsive force potential energy formula:
Wherein U is rep (q) represents repulsive potential energy, η represents repulsive scale factor, d 0 And the influence distance of the obstacle is represented, namely when the distance between the tool center point of the mechanical arm and the obstacle is larger than the influence distance of the obstacle, the repulsive potential energy received by the tool center point of the mechanical arm is 0.
The target mapping point is used as the coordinate of the target mapping point which needs to be reached by the tool center point of the mechanical arm, so that the step needs to establish the gravitational field of the target mapping point according to the gravitational potential energy formula:
wherein U is att (q) represents gravitational potential energy, S represents gravitational scale factor, d (q, q) goal ) Representing the initial position point coordinates of the mechanical arm and the targetThe distance of the coordinates of the points is mapped.
And then according to the sum potential energy formula:
U sum (q)＝U att (q)+U rep (q)
wherein U is sum (q) represents gravitational potential energy U att (q) repulsive potential energy U rep And (q).
And calculating the repulsive potential energy received by the tool center point of each predicted mechanical arm pose in the step 202 by using the repulsive potential energy formula, calculating the gravitational potential energy received by the tool center point of each predicted mechanical arm pose in the step 202 by using the gravitational potential energy formula, and comprehensively calculating the sum potential energy of the repulsive potential energy received by the tool center point of each predicted mechanical arm pose and the gravitational potential energy to obtain a plurality of sum potential energies. When the predicted mechanical arm pose corresponding to the m predicted joint angle combinations is present, m kinds of potential energy and m kinds of potential energy are obtained in the step.
205. And selecting the predicted mechanical arm pose with the minimum potential energy from m types of potential energy and the potential energy as the target mechanical arm pose of the next movement of the mechanical arm.
The magnitude values of the m types and the potential energy obtained in the step 204 can reflect the energy saving degree of the m types of predicted joint angle combinations when the mechanical arm is executed, and the potential energy is larger, and the relative energy consumption is possible to be larger, so that the predicted mechanical arm pose with the smallest potential energy is selected from the m types and the potential energy in the step and is used as the target mechanical arm pose of the mechanical arm moving in the next step, and the mechanical arm is more energy-saving and environment-friendly.
206. Judging whether the coordinates corresponding to the tool center point of the pose of the target mechanical arm are equal to the coordinates of the target mapping point, and if the coordinates corresponding to the tool center point of the pose of the target mechanical arm are equal to the coordinates of the target mapping point, executing step 207; if the coordinates corresponding to the tool center point of the pose of the target manipulator are not equal to the coordinates of the target mapping point, step 208 is performed.
After the pose of the target mechanical arm is obtained in step 205, whether the coordinates corresponding to the tool center point of the pose of the target mechanical arm obtained in step 205 are equal to the coordinates of the target mapping points is required to be judged, because the coordinates corresponding to the tool center point of the pose of the target mechanical arm, which moves in each step of the mechanical arm, are close to the coordinates of the target mapping points under the action of the sum potential energy in step 203, when the coordinates corresponding to the tool center point of the pose of the target mechanical arm are equal to the coordinates of the target mapping points, the movement of the tool center point of the mechanical arm can be stopped, and the fact that the tool center point of the mechanical arm has moved from the coordinates of the initial position point to the coordinates of the target mapping points is indicated; and when the coordinates corresponding to the tool center point of the pose of the target mechanical arm are not equal to the coordinates of the target mapping point, continuing moving, wherein the fact that the tool center point of the mechanical arm does not finish moving from the coordinates of the initial position point to the coordinates of the target mapping point is indicated.
207. And connecting the initial position point coordinates with the coordinates corresponding to the tool center points of all the target mechanical arm pose, and obtaining a first section of path.
When it is determined in step 206 that the coordinates corresponding to the tool center points of the pose of the target mechanical arm are equal to the coordinates of the target mapping points, indicating that the tool center points of the mechanical arm have completed moving from the initial position point coordinates to the target mapping point coordinates, the step connects the initial position point coordinates with the coordinates (one or more) corresponding to the tool center points of all the pose of the target mechanical arm, and a first path is obtained.
208. And taking the pose of the target mechanical arm as the current pose of the mechanical arm.
When it is determined in step 206 that the coordinates corresponding to the tool center point of the pose of the target mechanical arm are not equal to the coordinates of the target mapping point, which indicates that the tool center point of the mechanical arm does not move from the coordinates of the initial position point to the coordinates of the target mapping point, in order to enable the mechanical arm to continue moving, the step is to consider the pose of the target mechanical arm as the pose of the current mechanical arm, and trigger to execute step 202, so as to realize the next movement planning of the mechanical arm.
Therefore, in this embodiment, the potential energy minimum principle is utilized to screen out the most economical and energy-saving pose of the target mechanical arm in each step of movement, and the joint angle combination of the pose of the target mechanical arm is called as the optimal joint angle combination, so that the tool center point of the mechanical arm moves step by step from the initial position point coordinate to the target mapping point coordinate in the optimal joint angle combination mode, and the planning execution of the first path is completed in an energy-saving and efficient manner.
Referring to fig. 3, in step 204 of the embodiment of fig. 2, the process of calculating the first path of the tool center point of the mechanical arm moving from the initial position point coordinate to the target mapping point coordinate by using the artificial potential field method may specifically include:
301. judging whether the model data of the mechanical arm for predicting the pose of the mechanical arm has an intersection with the geometric envelope box, and executing step 302 if the model data of the mechanical arm for predicting the pose of the mechanical arm has an intersection with the geometric envelope box; if the model data of the robot arm predicting the pose of the robot arm does not intersect with the geometry envelope box, step 303 is performed.
Specifically, whether the model data of the mechanical arm predicting the pose of the mechanical arm in step 202 is intersected with the geometric envelope box or not is determined so as to determine whether the mechanical arm collides with the geometric envelope box (the head of a patient), if the model data of the mechanical arm predicting the pose of the mechanical arm is not intersected with the geometric envelope box, the joint angle combination corresponding to the pose of the mechanical arm is indicated to be executable; if the model data of the mechanical arm for predicting the mechanical arm pose and the geometric body envelope box are intersected, the joint angle combinations corresponding to the mechanical arm pose are indicated to be incapable of being executed, and if the risk exists, the collision joint angle combinations corresponding to the predicted mechanical arm pose for predicting the geometric body envelope box are eliminated. In other embodiments, the step determines whether the distance from the point on the equivalent link of the mechanical arm to the geometric envelope box (cube envelope box) is greater than the radius of the equivalent cylinder of the mechanical arm, if not, collision occurs; if the number is larger than the threshold value, no collision occurs.
302. And eliminating collision joint angle combinations corresponding to the predicted mechanical arm pose where the geometric envelope box is intersected.
If step 301 determines that the intersection exists between the model data of the mechanical arm for predicting the pose of the mechanical arm and the geometric envelope box, eliminating the joint angle combinations corresponding to the pose of the dangerous mechanical arm which cannot be executed, collecting and summarizing only the joint angle combinations corresponding to the pose of the mechanical arm which can be executed in step 301, and obtaining the joint angle combinations which cannot collide with the head of the patient, so that each step of the pose of the target mechanical arm of the first-stage path in the embodiment needs to be selectively executed in the joint angle combinations which cannot collide.
303. And summarizing all joint angle combinations corresponding to the predicted mechanical arm pose without intersection, and obtaining m joint angle combinations in which the measured mechanical arm pose cannot collide.
If step 301 determines that the model data of the mechanical arm for predicting the pose of the mechanical arm does not have an intersection with the geometric envelope box, it indicates that the joint angle combinations corresponding to the pose of the mechanical arm are all joint angle combinations that do not collide, and the joint angle combinations corresponding to the pose of the mechanical arm in step 301 are collected and summarized to obtain m joint angle combinations that do not collide with the head of the patient, so that each step of the target mechanical arm pose of the first path in the above embodiment needs to be selectively executed in the joint angle combinations that do not collide.
The foregoing embodiments describe a path planning method for a transcranial magnetic stimulation navigation process of the present application, and the following describes a path planning system for a transcranial magnetic stimulation navigation process of the present application, referring to fig. 4, and one embodiment of the path planning system for a transcranial magnetic stimulation navigation process is applied to a mechanical arm, and includes:
a determining unit 401, configured to determine an initial position point coordinate of a tool center point of the mechanical arm and a target magnetic stimulation point coordinate of a head of a patient;
a setting unit 402, configured to set a geometric envelope box for the patient's head, where the geometric envelope box wraps all magnetic stimulation point coordinates of the patient's head;
a mapping unit 403, configured to project the target magnetic stimulation point coordinates onto the surface of the geometric envelope box, so as to obtain target mapping point coordinates of the target magnetic stimulation point coordinates on the surface of the geometric envelope box;
a calculating unit 404, configured to calculate a first path of the tool center point of the mechanical arm moving from the initial position point coordinate to the target mapping point coordinate by using an artificial potential field method;
the calculating unit 404 is further configured to calculate a second path of the tool center point of the mechanical arm moving from the target mapping point coordinate to the target magnetic stimulation point coordinate by using a mechanical arm inverse kinematics solving algorithm;
And the connection unit 405 is configured to connect the first path section and the second path section in sequence, so as to obtain a planned path of the mechanical arm.
Optionally, when the setting unit 402 sets a geometric envelope box on the head of the patient, the setting unit specifically includes:
and performing cubic surrounding calculation on the head of the patient by using a surrounding box algorithm to obtain a cubic enveloping box for the head of the patient.
Optionally, the mapping unit 403 projects the target magnetic stimulation point coordinates onto the surface of the geometric envelope box, so as to obtain the target mapping point coordinates of the target magnetic stimulation point coordinates on the surface of the geometric envelope box, which is specifically configured to:
calculating the center point coordinates of the cube envelope box;
taking the center point coordinates of the cubic envelope box as the head center point coordinates of the head of the patient;
connecting the head center point coordinate with the target magnetic stimulation point coordinate to obtain a straight line L, wherein the straight line L intersects the cube envelope box at two intersection points;
and taking one of the two intersection points, which is close to the target magnetic stimulation point coordinate, as the target mapping point coordinate of the target magnetic stimulation point coordinate on the surface of the geometric envelope box.
Optionally, the calculating unit 404 calculates, using an artificial potential field method, a first path of the tool center point of the mechanical arm from the initial position point coordinate to the target mapping point coordinate, where the first path is specifically configured to:
determining the current mechanical arm pose of the mechanical arm;
calculating the predicted mechanical arm pose corresponding to the k joints of the mechanical arm when the k joints rotate by-lambda degrees, 0 degrees and lambda degrees respectively, wherein the predicted mechanical arm pose corresponds to 3 k Seed predictionJoint angle combination, wherein lambda is any value from 0 to 360;
calculating the sum potential energy of the tool center points of each predicted mechanical arm pose to obtain 3 k Seed and potential energy;
from said 3 k The predicted mechanical arm pose with the minimum seed and potential energy is selected as the target mechanical arm pose of the mechanical arm moving next step;
judging whether the coordinates corresponding to the tool center point of the pose of the target mechanical arm are equal to the coordinates of the target mapping point;
if the coordinates corresponding to the tool center points of the target mechanical arm pose are equal to the coordinates of the target mapping points, connecting the initial position point coordinates with the coordinates corresponding to the tool center points of all the target mechanical arm poses to obtain the first section of path;
And if the coordinates corresponding to the tool center point of the target mechanical arm pose are not equal to the coordinates of the target mapping point, regarding the target mechanical arm pose as the current mechanical arm pose, and triggering and executing the step of calculating the predicted mechanical arm pose corresponding to the k joints of the mechanical arm when the k joints rotate by-lambda degrees, 0 degrees and lambda degrees respectively.
Optionally, the system further comprises:
a detection unit 406, configured to perform collision detection on the predicted pose of the mechanical arm, determine m joint angle combinations where the predicted pose of the mechanical arm cannot collide, where m is greater than 0 and less than or equal to 3 k Is a positive integer of (2);
the calculation unit calculates the sum potential energy of the tool center points of each predicted mechanical arm pose to obtain 3 k The method is particularly used for the preparation of the composite material:
and calculating the sum potential energy of the center points of the m tools for predicting the pose of the mechanical arm to obtain m kinds of sum potential energy.
Optionally, the detecting unit 406 performs collision detection on the predicted pose of the mechanical arm, and is specifically configured to:
judging whether the model data of the mechanical arm of each predicted mechanical arm pose has intersection with the geometric envelope box or not;
If the intersection exists, eliminating a collision joint angle combination corresponding to the predicted mechanical arm pose of the intersection of the geometric envelope box;
if the intersection does not exist, summarizing all joint angle combinations corresponding to the predicted mechanical arm pose without the intersection, and obtaining m joint angle combinations in which the mechanical arm pose is not collided.
Optionally, the model data of the mechanical arm includes coil model data of a magnetic stimulation coil, the magnetic stimulation coil is installed at the tail end of the mechanical arm in an adapting way, a tool center point of the mechanical arm is located on the free end surface of the magnetic stimulation coil, and a coordinate value of the tool center point of the mechanical arm is equal to a coordinate value corresponding to a coordinate of the free end center point of the mechanical arm plus a thickness value of the magnetic stimulation coil.
Optionally, when the calculating unit 404 calculates the sum potential energy of the tool center points of each predicted pose of the mechanical arm, the calculating unit is specifically configured to:
establishing a repulsive field of the geometric envelope box and a gravitational field of the target mapping point;
calculating repulsive potential energy of a tool center point of each predicted mechanical arm pose in the repulsive force field;
calculating gravitational potential energy received by a tool center point of each predicted mechanical arm pose in the gravitational field;
And comprehensively calculating the sum potential energy of the repulsive potential energy and the gravitational potential energy received by the tool center point of each predicted mechanical arm pose.
The operation performed by the path planning system of the transcranial magnetic stimulation navigation process according to the embodiments of the present application is similar to that performed in the embodiments of fig. 1, 2 and 3, and will not be described in detail herein.
Turning now to the description of the computer device of the embodiments of the present application, referring to fig. 5, one embodiment of the computer device of the embodiments of the present application includes:
the computer device 500 may include one or more processors (central processing units, CPU) 501 and memory 502, with one or more applications or data stored in the memory 502. Wherein the memory 502 is volatile storage or persistent storage. The program stored in memory 502 may include one or more modules, each of which may include a series of instruction operations in a computer device. Still further, the processor 501 may be configured to communicate with the memory 502 and execute a series of instruction operations in the memory 502 on the computer device 500. The computer device 500 may also include one or more network interfaces 503, one or more input/output interfaces 504, and/or one or more operating systems, such as Windows Server, mac OS, unix, linux, freeBSD, etc. The processor 501 may perform the operations performed in the embodiments shown in fig. 1 to 3, and detailed descriptions thereof are omitted herein.
In the several embodiments provided in the embodiments of the present application, it should be understood by those skilled in the art that the disclosed systems, apparatuses and methods may be implemented in other manners. For example, the apparatus embodiments described above are merely illustrative, e.g., the division of the units is merely a logical function division, and there may be additional divisions when actually implemented, e.g., multiple units or components may be combined or integrated into another system, or some features may be omitted or not performed.
In addition, each functional unit in each embodiment of the present application may be integrated in one processing unit, or each unit may exist alone physically, or two or more units may be integrated in one unit. The integrated units may be implemented in hardware or in software functional units. The integrated units, if implemented in the form of software functional units and sold or used as stand-alone products, may be stored in a computer readable storage medium. Based on such understanding, the technical solution of the present application may be embodied in essence or a part contributing to the prior art or all or part of the technical solution in the form of a software product stored in a storage medium, including several instructions to cause a computer device (which may be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the methods of the embodiments of the present application. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a read-only memory (ROM), a random access memory (RAM, random access memory), a magnetic disk, or an optical disk, or other various media capable of storing program codes.
The foregoing description of the preferred embodiments of the present application is not intended to be limiting, but is intended to cover any and all modifications, equivalents, or alternatives falling within the spirit and principles of the present application.

Claims (8)

1. The path planning method for the transcranial magnetic stimulation navigation process is characterized by being applied to a mechanical arm and comprising the following steps of:
determining initial position point coordinates of a tool center point of the mechanical arm and target magnetic stimulation point coordinates of the head of a patient;
setting a geometric envelope box for the head of the patient, wherein the geometric envelope box wraps all magnetic stimulation point coordinates of the head of the patient;
projecting the target magnetic stimulation point coordinates to the surface of the geometric envelope box to obtain target mapping point coordinates of the target magnetic stimulation point coordinates on the surface of the geometric envelope box;
calculating a first path of the tool center point of the mechanical arm moving from the initial position point coordinate to the target mapping point coordinate by using an artificial potential field method;
calculating a second path of the tool center point of the mechanical arm moving from the target mapping point coordinate to the target magnetic stimulation point coordinate by using a mechanical arm inverse kinematics solving algorithm;
Connecting the first section of path and the second section of path in sequence to obtain a planning path of the mechanical arm;
the providing a geometry envelope box for the patient's head includes:
performing cubic surrounding calculation on the head of the patient by using a surrounding box algorithm to obtain a cubic surrounding box for the head of the patient;
the projecting the target magnetic stimulation point coordinates onto the surface of the geometric envelope box, and obtaining the target mapping point coordinates of the target magnetic stimulation point coordinates on the surface of the geometric envelope box comprises the following steps:
calculating the center point coordinates of the cube envelope box;
taking the center point coordinates of the cubic envelope box as the head center point coordinates of the head of the patient;
connecting the head center point coordinate with the target magnetic stimulation point coordinate to obtain a straight line L, wherein the straight line L intersects the cube envelope box at two intersection points;
and taking one of the two intersection points, which is close to the target magnetic stimulation point coordinate, as the target mapping point coordinate of the target magnetic stimulation point coordinate on the surface of the geometric envelope box.
2. The path planning method of claim 1, wherein calculating a first path of movement of a tool center point of the robotic arm from the initial position point coordinates to the target map point coordinates using an artificial potential field method comprises:
Determining the current mechanical arm pose of the mechanical arm;
calculating the predicted mechanical arm pose corresponding to the k joints of the mechanical arm when the k joints rotate by-lambda degrees, 0 degrees and lambda degrees respectively, wherein the predicted mechanical arm pose corresponds toA combination of predicted joint angles, wherein lambda is any value from 0 to 360;
calculating the sum potential energy of the tool center points of each predicted mechanical arm pose to obtainSeed and potential energy;
from the saidThe predicted mechanical arm pose with the minimum seed and potential energy is selected as the target mechanical arm pose of the mechanical arm moving next step;
judging whether the coordinates corresponding to the tool center point of the pose of the target mechanical arm are equal to the coordinates of the target mapping point;
if the coordinates corresponding to the tool center points of the target mechanical arm pose are equal to the coordinates of the target mapping points, connecting the initial position point coordinates with the coordinates corresponding to the tool center points of all the target mechanical arm poses to obtain the first section of path;
and if the coordinates corresponding to the tool center point of the target mechanical arm pose are not equal to the coordinates of the target mapping point, regarding the target mechanical arm pose as the current mechanical arm pose, and triggering and executing the step of calculating the predicted mechanical arm pose corresponding to the k joints of the mechanical arm when the k joints rotate by-lambda degrees, 0 degrees and lambda degrees respectively.
3. The path planning method of claim 2, wherein prior to calculating the sum potential energy of the tool center points for each predicted robot pose, the method further comprises:
performing collision detection on the predicted mechanical arm pose, and determining that the predicted mechanical arm pose cannot collideAngle of individual joints combination, said->Is greater than 0 and less than or equal to->Is a positive integer of (2);
the sum potential energy of the tool center points of each predicted mechanical arm pose is calculated to obtainThe seed and potential energy include：
Calculation ofThe sum potential energy of the tool center points of the predicted mechanical arm pose is obtained>Species and potential energy.
4. A path planning method according to claim 3, wherein said collision detection is performed on said predicted robot pose, and it is determined that said predicted robot pose does not collideThe individual joint angle combinations include:
judging whether the model data of the mechanical arm of each predicted mechanical arm pose has intersection with the geometric envelope box or not;
if the intersection exists, eliminating a collision joint angle combination corresponding to the predicted mechanical arm pose of the intersection of the geometric envelope box;
if the intersection does not exist, summarizing all joint angle combinations corresponding to the predicted mechanical arm pose without the intersection to obtain the situation that the measured mechanical arm pose does not collide The joint angles are combined.
5. The path planning method according to claim 4, wherein the model data of the mechanical arm comprises coil model data of a magnetic stimulation coil, the magnetic stimulation coil is adaptively mounted at the tail end of the mechanical arm, a tool center point of the mechanical arm is located on the free end surface of the magnetic stimulation coil, and a coordinate value of the tool center point of the mechanical arm is equal to a coordinate value corresponding to a coordinate value of the free end center point of the mechanical arm plus a thickness value of the magnetic stimulation coil.
6. The path planning method of claim 2, wherein calculating the sum potential energy of the tool center points of each predicted robot pose comprises:
establishing a repulsive field of the geometric envelope box and a gravitational field of the target mapping point;
calculating repulsive potential energy of a tool center point of each predicted mechanical arm pose in the repulsive force field;
calculating gravitational potential energy received by a tool center point of each predicted mechanical arm pose in the gravitational field;
and comprehensively calculating the sum potential energy of the repulsive potential energy and the gravitational potential energy received by the tool center point of each predicted mechanical arm pose.
7. A path planning system for a transcranial magnetic stimulation navigation procedure, applied to a robotic arm, comprising:
The determining unit is used for determining initial position point coordinates of a tool center point of the mechanical arm and target magnetic stimulation point coordinates of the head of the patient;
a setting unit for setting a geometric envelope box for the head of the patient, the geometric envelope box wrapping all magnetic stimulation point coordinates of the head of the patient;
the mapping unit is used for projecting the target magnetic stimulation point coordinates to the surface of the geometric envelope box to obtain target mapping point coordinates of the target magnetic stimulation point coordinates on the surface of the geometric envelope box;
the calculation unit is used for calculating a first section path of the tool center point of the mechanical arm moving from the initial position point coordinate to the target mapping point coordinate by using an artificial potential field method;
the calculation unit is further used for calculating a second section path of the tool center point of the mechanical arm moving from the target mapping point coordinate to the target magnetic stimulation point coordinate by utilizing a mechanical arm inverse kinematics solving algorithm;
the connecting unit is used for connecting the first section path and the second section path in sequence to obtain a planned path of the mechanical arm;
the setting unit, when setting a geometry envelope box for the patient's head, specifically comprises:
Performing cubic surrounding calculation on the head of the patient by using a surrounding box algorithm to obtain a cubic surrounding box for the head of the patient;
the mapping unit projects the target magnetic stimulation point coordinates to the surface of the geometric envelope box, and the mapping unit is specifically used for, when obtaining the target mapping point coordinates of the target magnetic stimulation point coordinates on the surface of the geometric envelope box:
calculating the center point coordinates of the cube envelope box;
taking the center point coordinates of the cubic envelope box as the head center point coordinates of the head of the patient;
connecting the head center point coordinate with the target magnetic stimulation point coordinate to obtain a straight line L, wherein the straight line L intersects the cube envelope box at two intersection points;
and taking one of the two intersection points, which is close to the target magnetic stimulation point coordinate, as the target mapping point coordinate of the target magnetic stimulation point coordinate on the surface of the geometric envelope box.
8. A computer device, comprising:
processor, memory, bus, input/output interface, network interface;
the processor is connected with the memory, the input/output interface and the network interface through buses;
The memory stores a program;
the processor, when executing the program stored in the memory, implements a path planning method of a transcranial magnetic stimulation navigation procedure according to any one of claims 1 to 6.
CN202210118146.0A 2022-02-08 2022-02-08 Path planning method and related device for transcranial magnetic stimulation navigation process Active CN114376726B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210118146.0A CN114376726B (en) 2022-02-08 2022-02-08 Path planning method and related device for transcranial magnetic stimulation navigation process

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210118146.0A CN114376726B (en) 2022-02-08 2022-02-08 Path planning method and related device for transcranial magnetic stimulation navigation process

Publications (2)

Publication Number Publication Date
CN114376726A CN114376726A (en) 2022-04-22
CN114376726B true CN114376726B (en) 2024-03-29

Family

ID=81204915

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210118146.0A Active CN114376726B (en) 2022-02-08 2022-02-08 Path planning method and related device for transcranial magnetic stimulation navigation process

Country Status (1)

Country Link
CN (1) CN114376726B (en)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114986524B (en) * 2022-08-08 2022-10-28 中国科学院自动化研究所 Mechanical arm track planning method and device based on brain-computer interface and electronic equipment

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104029203A (en) * 2014-06-18 2014-09-10 大连大学 Path planning method for implementation of obstacle avoidance for space manipulators
CN108846891A (en) * 2018-05-30 2018-11-20 广东省智能制造研究所 A kind of man-machine safety collaboration method based on three-dimensional framework detection
CN110896609A (en) * 2018-09-27 2020-03-20 武汉资联虹康科技股份有限公司 TMS positioning navigation method for transcranial magnetic stimulation treatment
CN111665842A (en) * 2020-06-09 2020-09-15 山东大学 Indoor SLAM mapping method and system based on semantic information fusion
CN113017625A (en) * 2021-02-26 2021-06-25 北京伟浩君智能技术有限公司 Control method and device of blood sampling robot

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7520848B2 (en) * 2004-04-09 2009-04-21 The Board Of Trustees Of The Leland Stanford Junior University Robotic apparatus for targeting and producing deep, focused transcranial magnetic stimulation
US20100185042A1 (en) * 2007-08-05 2010-07-22 Schneider M Bret Control and coordination of transcranial magnetic stimulation electromagnets for modulation of deep brain targets
CN109260593B (en) * 2018-09-27 2020-09-08 武汉资联虹康科技股份有限公司 Transcranial magnetic stimulation treatment method and equipment

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104029203A (en) * 2014-06-18 2014-09-10 大连大学 Path planning method for implementation of obstacle avoidance for space manipulators
CN108846891A (en) * 2018-05-30 2018-11-20 广东省智能制造研究所 A kind of man-machine safety collaboration method based on three-dimensional framework detection
CN110896609A (en) * 2018-09-27 2020-03-20 武汉资联虹康科技股份有限公司 TMS positioning navigation method for transcranial magnetic stimulation treatment
CN111665842A (en) * 2020-06-09 2020-09-15 山东大学 Indoor SLAM mapping method and system based on semantic information fusion
CN113017625A (en) * 2021-02-26 2021-06-25 北京伟浩君智能技术有限公司 Control method and device of blood sampling robot

Also Published As

Publication number Publication date
CN114376726A (en) 2022-04-22

Similar Documents

Publication Publication Date Title
JP6530759B2 (en) System and method for dynamic virtual collision object
CN104334110B (en) Avoid manipulator arm using kernel to collide with patient
JP5114019B2 (en) Method for controlling the trajectory of an effector
JPH09109072A (en) Control method for redundant manipulator
CN114376726B (en) Path planning method and related device for transcranial magnetic stimulation navigation process
CN114227672B (en) Mechanical arm safety collision track planning method and device, storage medium and equipment
Patel et al. A collision‐avoidance scheme for redundant manipulators: Theory and experiments
CN106844951B (en) Method and system for solving inverse kinematics of super-redundant robot based on segmented geometric method
CN115229805B (en) Hand-eye calibration method and device for surgical robot, storage medium and processor
CN113742992B (en) Master-slave control method based on deep learning and application
Kondo Inverse kinematics of a human arm
CN111358659B (en) Robot power-assisted control method and system and lower limb rehabilitation robot
CN112917479B (en) Approximate pose calculation method and device of five-axis robot and storage medium
Gienger et al. Exploiting task intervals for whole body robot control
Li et al. Synthesizing redundancy resolution criteria of the human arm posture in reaching movements
Kapusta et al. Task-centric selection of robot and environment initial configurations for assistive tasks
Melo et al. Adaptive admittance control to generate real-time assistive fixtures for a cobot in transpedicular fixation surgery
JPWO2020188064A5 (en)
Adhami et al. Positioning tele-operated surgical robots for collision-free optimal operation
Kuang et al. Research on biomimetic coordination action of service robot based on stereo vision
Wu et al. A New Geometric Method for Solving the Inverse Kinematics of Two-Segment Continuum Robot
CN114504735A (en) Path planning method and related device for transcranial magnetic stimulation navigation process
CN110877335A (en) Self-adaptive unmarked mechanical arm track tracking method based on hybrid filter
CN109719733A (en) A kind of multi-arm robot's mechanical arm interference analysis method
JP2000112510A (en) Robot teaching method and its device

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
TA01 Transfer of patent application right

Effective date of registration: 20240226

Address after: Building 9, Zone 2, Technology Enterprise Accelerator, No. 6 Qinling Avenue West, Caotang Technology Industrial Base, High-tech Zone, Xi'an City, Shaanxi Province 710001 20201

Applicant after: Xi'an KeYue Medical Technology Co.,Ltd.

Country or region after: China

Address before: 710000 9-30301, building 9, 2 West Qinling Avenue, Caotang technology enterprise accelerator, high tech Zone, Xi'an City, Shaanxi Province

Applicant before: Xi'an KeYue Medical Co.,Ltd.

Country or region before: China

TA01 Transfer of patent application right
GR01 Patent grant
GR01 Patent grant