Disclosure of Invention
The invention aims to provide a readable storage medium, a surgical robot system and an adjusting system, which can realize the purpose of automatically adjusting the position of a patient and the pose of a mechanical arm under the condition that an instrument is not withdrawn.
To achieve the above object, the present invention provides a readable storage medium for use in a surgical robot system, the readable storage medium storing a computer program which, when executed by a processor, performs the steps of:
receiving an adjustment instruction of a user;
acquiring a target pose of the mechanical arm or a target pose of the supporting device;
According to the target pose of the mechanical arm, controlling the mechanical arm to adjust and control the supporting device to move correspondingly along with the mechanical arm, or
And controlling the supporting device to carry out adjustment movement and controlling the mechanical arm to carry out corresponding movement along with the supporting device according to the target pose of the supporting device.
Optionally, the acquiring the target pose of the mechanical arm or the target pose of the supporting device includes:
acquiring the target pose of the mechanical arm or the target pose of the supporting device according to the corresponding relation between the pre-stored target pose and the operation type, or
And acquiring the target pose of the mechanical arm or the target pose of the supporting device according to a preset target function.
Optionally, the obtaining the target pose of the mechanical arm or the target pose of the supporting device according to the preset objective function includes:
Taking one mechanical arm as a target mechanical arm;
Acquiring the current position of a stationary point of the target mechanical arm;
Creating a safety space according to the current position of the fixed point of the target mechanical arm;
Traversing each point of the safety space, and solving the function value of a preset objective function at different positions;
taking the position of which the function value meets the preset condition as the target position of the motionless point of the target mechanical arm;
And acquiring the target pose of the mechanical arm or the target pose of the supporting device according to the target position of the fixed point of the target mechanical arm.
Optionally, the preset objective function is:
w(q)=α·w1(q)+β·w2(q)
Wherein alpha is the weight of w1 (q), beta is the weight of w2 (q), alpha+beta=1, N is the number of joints of the target mechanical arm, qi is the position of the ith joint of the target mechanical arm when traversing the safety space,Qimax is the average position of the ith joint of the target mechanical arm, qimin is the maximum position of the ith joint of the target mechanical arm, qimin is the minimum position of the ith joint of the target mechanical arm, n is the number of mechanical arms of the robot, hi is the distance between two adjacent mechanical arms when traversing the safety space,Is the average value of the spacing between all adjacent mechanical arms;
the step of taking the position of the function value meeting the preset condition as the target position of the fixed point of the target mechanical arm includes:
and taking the position when the function value is the maximum as the target position of the fixed point of the target mechanical arm.
Optionally, according to the target pose of the mechanical arm, controlling the mechanical arm to perform adjustment movement and controlling the supporting device to perform corresponding movement along with the mechanical arm, including:
acquiring the current pose of the mechanical arm;
Planning a motion track of the mechanical arm according to the target pose and the current pose of the mechanical arm;
According to the planned motion trail of the mechanical arm, controlling the mechanical arm to adjust motion according to the motion trail, and controlling the supporting device to correspondingly move along with the motion trail of the mechanical arm;
according to the target pose of the supporting device, controlling the supporting device to perform adjustment movement and controlling the mechanical arm to perform corresponding movement along with the supporting device, including:
acquiring the current pose of the supporting device;
Planning a motion track of the supporting device according to the target pose and the current pose of the supporting device;
And controlling the supporting device to adjust according to the planned movement track of the supporting device, and controlling the mechanical arm to move correspondingly along with the movement track of the supporting device.
Optionally, the planning the motion track of the mechanical arm according to the target pose and the current pose of the mechanical arm includes:
according to the target pose and the current pose of the mechanical arm, an interpolation algorithm is adopted to obtain the motion trail of the mechanical arm;
Planning a motion trail of the supporting device according to a target pose and a current pose of the supporting device, including:
And according to the target pose and the current pose of the supporting device, acquiring the motion trail of the supporting device by adopting an interpolation algorithm.
Optionally, the planning the motion track of the mechanical arm according to the target pose and the current pose of the mechanical arm includes:
Planning the motion trail of each joint of the mechanical arm according to the target position and the current position of each joint of the mechanical arm;
Planning a motion trail of the supporting device according to a target pose and a current pose of the supporting device, including:
and planning the motion trail of each joint of the supporting device according to the target position and the current position of each joint of the supporting device.
Optionally, the controlling the supporting device to follow the mechanical arm to perform corresponding movement includes:
Acquiring real-time position information of a stationary point of the mechanical arm under a robot coordinate system;
Acquiring a target mapping relation between a support device coordinate system and a world coordinate system in real time according to real-time position information of the motionless point under the robot coordinate system, the mapping relation between the motionless point and the support device coordinate system and the mapping relation between the robot coordinate system and the world coordinate system;
According to a target mapping relation between the support device coordinate system and the world coordinate system, which is obtained in real time, controlling the support device to move correspondingly along with the mechanical arm;
the control of the mechanical arm to follow the supporting device to perform corresponding movement comprises the following steps:
acquiring a real-time mapping relation between a supporting device coordinate system and a world coordinate system;
acquiring target position information of the stationary point of the mechanical arm under the robot coordinate system in real time according to a real-time mapping relation between the support device coordinate system and the world coordinate system, a mapping relation between the stationary point of the mechanical arm and the support device coordinate system, and a mapping relation between the robot coordinate system and the world coordinate system;
And controlling the mechanical arm to follow the supporting device to perform corresponding movement according to the real-time acquired target position information of the fixed point of the mechanical arm under the robot coordinate system.
Optionally, when the computer program is executed by a processor, the following steps are implemented:
and judging whether the current state of the surgical robot system meets the adjustment requirement.
Optionally, when the computer program is executed by a processor, the following steps are implemented:
And monitoring the real-time adjustment movement process of the mechanical arm and the supporting device to judge whether an abnormal condition occurs.
Optionally, when the computer program is executed by a processor, the following steps are implemented:
And displaying the real-time adjustment movement process of the mechanical arm and the supporting device.
To achieve the above object, the present invention also provides a surgical robot system comprising a robot comprising at least one mechanical arm and a controller in communication with the robot, the controller comprising a processor and a readable storage medium as described above.
Optionally, the surgical robot system includes a display device communicatively connected to the controller, where the display device is configured to display a real-time adjustment motion process of the mechanical arm and the support device.
In order to achieve the above object, the present invention further provides an adjustment system, which includes the above-mentioned robot system and a positioning device, where the positioning device is configured to obtain a mapping relationship between a robot coordinate system and a world coordinate system and a mapping relationship between a support device coordinate system and the world coordinate system.
Optionally, the adjustment system includes a support device in communication with the controller, the controller configured to control the support device to perform the adjustment movement.
Compared with the prior art, the readable storage medium, the surgical robot system and the adjusting system have the advantages that the target pose of the mechanical arm of the robot is firstly obtained, then the mechanical arm is adjusted to move so as to adjust the pose of the mechanical arm to the target pose, and in the adjusting process of the mechanical arm, the supporting device is controlled to move correspondingly along with the mechanical arm, or the target pose of the supporting device is firstly obtained, then the supporting device is adjusted to move so as to adjust the pose of the supporting device to the target pose, and in the adjusting process of the supporting device, the mechanical arm of the robot is controlled to move correspondingly along with the supporting device. Therefore, the invention can realize the adjustment of the body position (namely the position of the supporting device) of the patient and the position of the mechanical arm under the condition of not withdrawing the instrument, thereby completing the operation more efficiently and safely, reducing the requirements on the position of punching before operation and the positioning of equipment, effectively reducing the preparation time before operation and effectively avoiding the collision probability of the mechanical arm.
Detailed Description
The readable storage medium, the surgical robotic system and the adjustment system according to the present invention are described in further detail below with reference to fig. 1 to 16 and the detailed description. The advantages and features of the present invention will become more apparent from the following description. It should be noted that the drawings are in a very simplified form and are all to a non-precise scale, merely for the purpose of facilitating and clearly aiding in the description of embodiments of the invention. For a better understanding of the invention with objects, features and advantages, refer to the drawings. It should be understood that the structures, proportions, sizes, etc. shown in the drawings are shown only in connection with the present disclosure for the understanding and reading of the present disclosure, and are not intended to limit the scope of the invention, which is defined by the appended claims, and any structural modifications, proportional changes, or dimensional adjustments, which may be made by the present disclosure, should fall within the scope of the present disclosure under the same or similar circumstances as the effects and objectives attained by the present invention.
It is noted that relational terms such as first and second, and the like are used solely to distinguish one entity or action from another entity or action without necessarily requiring or implying any actual such relationship or order between such entities or actions. Moreover, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising one does not exclude the presence of other like elements in a process, method, article, or apparatus that comprises an element.
The invention mainly aims to provide a readable storage medium, a surgical robot system and an adjusting system, which can realize the purpose of automatically adjusting the position of a patient and the pose of a mechanical arm under the condition that an instrument is not withdrawn. The robot coordinate system (X2, Y2, Z2) herein refers to a coordinate system created with any point on the robot base as an origin, the support device coordinate system (X1, Y1, Z1) refers to a coordinate system created with any point on the support body of the support device as an origin, the pose of the robot arm refers to a pose of the robot arm under the robot coordinate system (X2, Y2, Z2), the pose of the support device refers to a pose of the support device under the world coordinate system (X0, Y0, Z0), and the mapping relationship between the robot coordinate system (X2, Y2, Z2) and the world coordinate system (X0, Y0, Z0) is unchanged during the automatic adjustment, and the mapping relationship between the support device coordinate system (X1, Y1, Z1) and the world coordinate system (X0, Y0, Z0) changes with the change of the pose of the support device.
In order to achieve the above object, the present invention provides a readable storage medium applied to a surgical robot system including a robot including at least one mechanical arm, the readable storage medium storing a computer program. Referring to fig. 1, a schematic diagram of steps implemented when a computer program stored on a readable storage medium according to an embodiment of the present invention is executed by a processor is shown. As shown in fig. 1, the computer program, when executed by a processor, performs the steps of:
s1, receiving an adjustment instruction of a user;
S2, acquiring a target pose of the mechanical arm or a target pose of the supporting device;
And S3, controlling the mechanical arm to move and controlling the supporting device to move correspondingly along with the mechanical arm according to the target pose of the mechanical arm, or controlling the supporting device to adjust and controlling the mechanical arm to move correspondingly along with the supporting device according to the target pose of the supporting device.
The invention can adjust the pose of the mechanical arm of the robot to the target pose by firstly acquiring the target pose of the mechanical arm of the robot and then adjusting the mechanical arm to the target pose, and control the supporting device to move correspondingly along with the mechanical arm in the adjusting process of the mechanical arm, or can adjust the pose of the supporting device to the target pose by firstly acquiring the target pose of the supporting device and then adjusting the supporting device to move correspondingly along with the supporting device in the adjusting process of the supporting device. Therefore, the invention can realize the adjustment of the body position (namely the position of the supporting device) of the patient and the position of the mechanical arm under the condition of not withdrawing the instrument, thereby completing the operation more efficiently and safely, reducing the requirements on the position of punching before operation and the positioning of equipment, effectively reducing the preparation time before operation and effectively avoiding the collision probability of the mechanical arm.
Further, the acquiring the target pose of the mechanical arm or the target pose of the supporting device includes:
Acquiring the target pose of the mechanical arm or the target pose of the supporting device according to the corresponding relation between the pre-stored target pose and the operation type, or
And acquiring the target pose of the mechanical arm or the target pose of the supporting device according to a preset target function.
Referring to fig. 2, a schematic flow chart of acquiring a target pose according to an embodiment of the present application is shown. As shown in fig. 2, a user (for example, a medical staff) may select a corresponding operation type through an input unit (for example, a physical key or a virtual key) to enter a recovery mode, and at this time, may select a target pose of the mechanical arm or a target pose of the supporting device according to a pre-stored correspondence between a target pose and the operation type. The user (for example, medical staff) can also select to enter the setting mode through the physical key or the virtual key, and at this time, the target pose of the mechanical arm or the target pose of the supporting device can be obtained according to a preset objective function. The target pose of the mechanical arm in the present application refers to a pose of the mechanical arm (i.e., a terminal position and a pose of the mechanical arm) to be adjusted, and the target pose of the supporting device refers to a pose of the supporting device (i.e., a three-dimensional spatial position of the supporting device and a rotation angle around each direction) to be adjusted.
Specifically, the target pose of the mechanical arm or the target pose of the supporting device can be obtained according to a preset target function through the following steps:
Taking one mechanical arm as a target mechanical arm;
Acquiring the current position of a stationary point of the target mechanical arm;
Creating a safety space according to the current position of the fixed point of the target mechanical arm;
Traversing each point of the safety space, and solving the function value of a preset objective function at different positions;
taking the position of which the function value meets the preset condition as the target position of the motionless point of the target mechanical arm;
And acquiring the target pose of the mechanical arm or the target pose of the supporting device according to the target position of the fixed point of the target mechanical arm.
Referring to fig. 3, a schematic flow chart of resolving a pose of an acquisition target according to an embodiment of the invention is shown. As shown in fig. 3, in the actual operation process, any one mechanical arm may be selected as a target mechanical arm, for example, a mechanical arm with an endoscope is selected as a target mechanical arm, by acquiring the current position (coordinates under a robot coordinate system (X2, Y2, Z2)) of an motionless point of the target mechanical arm, creating a safety space (ensuring that the mechanical arm does not damage organ tissues of a patient when moving in the safety space) according to the current position of the motionless point of the target mechanical arm, where the safety space may be a three-dimensional space with smooth boundaries, such as a spherical space, an ellipsoidal space, a conical space, and the like, specifically, taking the current position of the motionless point as a spherical center and a preset radius (for example, 2 cm) as a radius, creating a safety space, traversing each point (including each point on the boundary of the safety space and each point in the safety space) of a fixed step length, solving a function value of a preset objective function in different positions, and taking a position satisfying a preset condition as a position of the motionless point of the target mechanical arm (coordinates of the motionless point, namely, the target robot 2) under the target coordinate system (Z2). And (3) obtaining the target position of each joint of the target mechanical arm by carrying out inverse solution on the target position of the fixed point, for example, adopting an inverse kinematics method. Because the fixed points of the other mechanical arms and the fixed points of the target mechanical arm have a preset mapping relation, according to the mapping relation between the fixed points of the other mechanical arms and the fixed points of the target mechanical arm and the target positions of the fixed points of the target mechanical arm, the target positions of the fixed points of the other mechanical arms, namely the target positions of the other mechanical arms, can be obtained, and the target positions of the joints of the other mechanical arms can be obtained by reversely solving the target positions of the fixed points of the other mechanical arms.
Similarly, since a predetermined mapping relationship is provided between the stationary point of the target mechanical arm and the coordinate system (X1, Y1, Z1) of the supporting device, and a predetermined mapping relationship is provided between the robot coordinate system (X2, Y2, Z2) and the world coordinate system (X0, Y0, Z0), the target position of each joint of the supporting device can be obtained by inversely solving the target position of the supporting device according to the target position of the stationary point of the target mechanical arm, the mapping relationship between the stationary point of the target mechanical arm and the coordinate system (X1, Y1, Z1) of the supporting device, and the mapping relationship between the robot coordinate system (X2, Y2, Z2) and the world coordinate system (X0, Y0, Z0). Wherein, the mapping relation between the robot coordinate system (X2, Y2, Z2) and the world coordinate system (X0, Y0, Z0) can be measured by the positioning device.
Further, the preset objective function is:
w(q)=α·w1(q)+β·w2(q)
Wherein alpha is the weight of w1 (q), beta is the weight of w2 (q), alpha+beta=1, N is the number of joints of the target mechanical arm, qi is the position of the ith joint of the target mechanical arm when traversing the safety space,Qimax is the average position of the ith joint of the target mechanical arm, qimin is the maximum position of the ith joint of the target mechanical arm, qimin is the minimum position of the ith joint of the target mechanical arm, n is the number of mechanical arms of the robot, hi is the distance between two adjacent mechanical arms when traversing the safety space,Is the average of the spacing between all adjacent robotic arms.
Correspondingly, the step of taking the position of the function value meeting the preset condition as the target position of the fixed point includes:
and taking the position when the objective function value is the maximum as the objective position of the fixed point.
Specifically, when traversing a certain point of the safety space, the positions of the joints of the target mechanical arm at the position and the corresponding positions of the fixed points of other mechanical arms at the position are obtained through an inverse kinematics algorithm, so that the function value of the target function w (q) at the position is obtained. Similarly, the function value of the objective function w (q) at the corresponding position can be obtained by the method when any other point of the safety space is traversed. And comparing the function values of the objective function w (q) at different positions, and taking the position when the function value is maximum as the target position of the stationary point of the target mechanical arm.
The specific values of α and β may be set according to the specific situation, for example, when α is 1 and β is 0, the objective function is:
For this case, when the function value of the objective function w (q) is maximum, qi approachesThat is, the positions of the joints of the mechanical arm 220 of the robot approach to the average positions thereof, so that the working space range of the mechanical arm is greatly increased, and the condition of optimal movement space of the mechanical arm can be satisfied.
When α is 0 and β is 1, the objective function is:
for this case, when the objective function value is maximum, hi approachesThe mechanical arms of the robot are distributed at equal intervals, so that collision among the mechanical arms in the operation process can be effectively avoided, and the condition that the mechanical arms are positioned optimally can be met.
When alpha is 0.5 and beta is 0.5, the optimization of the movement space of the mechanical arm and the optimization of the swing of the mechanical arm can be balanced, so that the effective movement space of the mechanical arm can be improved, and the collision probability of the mechanical arm in the operation process can be effectively reduced.
With continued reference to fig. 4, a schematic diagram of a flow of the mechanical arm following the supporting device to perform an adjustment motion is provided in an embodiment of the present invention, as shown in fig. 4, the flow of controlling the mechanical arm to perform an adjustment motion and controlling the supporting device to follow the mechanical arm to perform a corresponding motion according to a target pose of the mechanical arm includes:
acquiring the current pose of the mechanical arm;
Planning a motion track of the mechanical arm according to the target pose and the current pose of the mechanical arm;
according to the planned movement track of the mechanical arm, controlling the mechanical arm to carry out adjustment movement according to the movement track, and controlling the supporting device (such as an operating table) to carry out corresponding movement along the movement track of the mechanical arm.
Please continue to refer to fig. 5, which schematically illustrates a flow chart of the supporting device according to an embodiment of the present invention following the mechanical arm for adjustment movement. As shown in fig. 5, according to the target pose of the supporting device, the controlling the supporting device to perform an adjustment motion and controlling the mechanical arm to perform a corresponding motion along with the supporting device includes:
acquiring the current pose of the supporting device;
Planning a motion track of the supporting device according to the target pose and the current pose of the supporting device;
And controlling the supporting device to adjust according to the planned movement track of the supporting device, and controlling the mechanical arm to move correspondingly along with the movement track of the supporting device.
Further, the planning the motion track of the mechanical arm according to the target pose and the current pose of the mechanical arm includes:
according to the target pose and the current pose of the mechanical arm, an interpolation algorithm is adopted to obtain the motion trail of the mechanical arm;
Planning a motion trail of the supporting device according to a target pose and a current pose of the supporting device, including:
And according to the target pose and the current pose of the supporting device, acquiring the motion trail of the supporting device by adopting an interpolation algorithm.
Fig. 6 to 8 are schematic diagrams of a process of planning a motion track of a mechanical arm or a supporting device by interpolation of a trapezoidal speed curve according to an embodiment of the present invention, fig. 7 is a schematic diagram of a trapezoidal speed curve according to an embodiment of the present invention, and fig. 8 is a schematic diagram of a triangular speed curve according to an embodiment of the present invention. As shown in fig. 6 to 8, the speed rise time ts in the trapezoidal speed curve can be obtained from the set maximum speed Vmax and the constant acceleration a, which can be set according to the specific case. When the planned distance s (the distance between the target position and the current position) is smaller than Vmax*ts, a triangular speed curve is used to generate a motion track, where the speed track is shown in fig. 8, and the position track may be obtained by speed integration, where:
v=at,t≤0
v=a(t-2t0),t0≤t≤2t0
when the planned distance s is greater than or equal to Vmax*ts, a trapezoidal speed curve is used to generate a motion track, where the speed track is shown in fig. 7, and the position track may be obtained by speed integration, where:
v=at,t≤ts
v=Vmax,ts≤t≤tf-ts
v=-a(t-tf),tf-ts≤t≤tf
the principle of planning the motion trail by polynomial interpolation is described below by taking the polynomial interpolation as an example.
The positional expression at the time t is set as:
q(t)＝a5t5+a4t4+a3t3+a2t2+a1t+a0
setting the initial time position as q0, the end time position as qf, and the initial time speed and the end time speed as 0, wherein the conditions are constraint conditions, namely, the constraint conditions are as follows:
q(0)=qs
q(tf)＝qf
Using the 6 constraints described above, a position trajectory can be obtained in which:
a0＝qs
a1＝a2=0
The velocity trajectory can be obtained by deriving the position trajectory, and please refer to fig. 9, which schematically shows a velocity graph of a five-term interpolation planning trajectory according to an embodiment of the present invention. The acceleration track can be obtained by deriving the velocity track, please refer to fig. 10, which schematically shows an acceleration graph of the five-term interpolation planning track according to an embodiment of the present invention.
In an exemplary embodiment, the planning the motion trajectory of the mechanical arm according to the target pose and the current pose of the mechanical arm includes:
Planning the motion trail of each joint of the mechanical arm according to the target position and the current position of each joint of the mechanical arm;
Planning a motion trail of the supporting device according to a target pose and a current pose of the supporting device, including:
and planning the motion trail of each joint of the supporting device according to the target position and the current position of each joint of the supporting device.
Further, the controlling the supporting device to follow the mechanical arm to perform corresponding movement includes:
Acquiring real-time position information of a stationary point of the mechanical arm under a robot coordinate system;
Acquiring a target mapping relation between a support device coordinate system and a world coordinate system in real time according to real-time position information of the motionless point under the robot coordinate system, the mapping relation between the motionless point and the support device coordinate system and the mapping relation between the robot coordinate system and the world coordinate system;
and controlling the supporting device to follow the mechanical arm to perform corresponding movement according to the target mapping relation between the supporting device coordinate system and the world coordinate system, which are acquired in real time.
Specifically, in the process that the supporting device follows the mechanical arm j to perform corresponding movement, the target mapping relation between the coordinate system (X1, Y1, Z1) of the supporting device and the world coordinate system (X0, Y0, Z0) is formedThe method can be obtained by the following formula:
Wherein, theThe mapping relation between the robot coordinate system (X2, Y2, Z2) and the world coordinate system (X0, Y0, Z0) is2Pi the coordinates of the stationary point of the mechanical arm j in the robot coordinate system (X2, Y2, Z2), which continuously changes along with the adjustment movement of the mechanical arm j,1Pj the coordinates of the stationary point of the mechanical arm j in the support device coordinate system (X1, Y1, Z1), and (1Pj)+ the pseudo inverse of1Pj.
Because the position sensors are mounted on the joints of the mechanical arm j, in the adjustment movement process of the mechanical arm 220, the coordinates of the joints in the robot coordinate system (X2, Y2, Z2) can be obtained in real time, and the coordinates2Pj of the stationary point of the mechanical arm j in the robot coordinate system (X2, Y2, Z2) can be obtained in real time through a kinematic equation. Thus, according to the above formula, the target mapping relationship between the supporting device coordinate system (X1, Y1, Z1) and the world coordinate system (X0, Y0, Z0) can be obtained in real timeAnd further can be based on a target mapping between the support device coordinate system (X1, Y1, Z1) and the world coordinate system (X0, Y0, Z0) acquired in real timeAnd controlling the supporting device to follow the mechanical arm j to perform corresponding movement. Specifically, the target mapping relation between the acquired supporting device coordinate system (X1, Y1, Z1) and the world coordinate system (X0, Y0, Z0) can be realizedAnd acquiring the target positions of all joints of the supporting device by adopting an inverse kinematics method, and further controlling the supporting device to follow the mechanical arm j to perform corresponding movement according to the real-time acquired target positions of all joints of the supporting device.
The control of the mechanical arm to follow the supporting device to perform corresponding movement comprises the following steps:
acquiring a real-time mapping relation between a supporting device coordinate system and a world coordinate system;
acquiring target position information of the stationary point of the mechanical arm under the robot coordinate system in real time according to a real-time mapping relation between the support device coordinate system and the world coordinate system, a mapping relation between the stationary point of the mechanical arm and the support device coordinate system, and a mapping relation between the robot coordinate system and the world coordinate system;
And controlling the mechanical arm to follow the supporting device to perform corresponding movement according to the real-time acquired target position information of the fixed point of the mechanical arm under the robot coordinate system.
Specifically, taking the mechanical arm j as an example, in the process that the mechanical arm j follows the supporting device to perform adjustment movement, the target coordinate2P′j of the fixed point of the mechanical arm j in the robot coordinate system (X2, Y2, Z2) can be obtained by the following formula:
Wherein, theIs the mapping relation between the coordinate system (X1, Y1, Z1) of the supporting device and the world coordinate system (X0, Y0, Z0), which is continuously changed along with the adjusting movement of the supporting device,Is the mapping relation between the robot coordinate system (X2, Y2, Z2) and the world coordinate system (X0, Y0, Z0),1Pj is the coordinate of the stationary point of the mechanical arm j under the support device coordinate system (X1, Y1, Z1).
In the adjustment movement of the supporting device, the mapping relation between the coordinate system (X1, Y1, Z1) of the supporting device and the world coordinate system (X0, Y0, Z0) can be measured in real time by the positioning deviceTherefore, according to the above formula, the target coordinates2Pj 'of the stationary point of the mechanical arm j in the robot coordinate system (X2, Y2, Z2) can be obtained in real time, and then the mechanical arm j can be controlled to follow the supporting device 100 to perform the adjustment motion according to the target coordinates2Pj' of the stationary point in the robot coordinate system (X2, Y2, Z2) obtained in real time. Specifically, the target positions of the joints of the mechanical arm j may be obtained by using an inverse kinematics solution according to the target coordinates2Pj' of the stationary point in the robot coordinate system (X2, Y2, Z2), and then the mechanical arm j may be controlled to move correspondingly along with the supporting device 100 according to the target positions of the joints of the mechanical arm j obtained in real time. The movement process of the other mechanical arm 220 may refer to the adjustment process of the mechanical arm j, so that the description thereof will not be repeated here.
Fig. 11 is a schematic diagram showing a flow chart of automatic adjustment of the mechanical arm and the supporting device according to an embodiment of the invention. As shown in fig. 11, the computer program, when executed by a processor, further performs the steps of:
and judging whether the current state of the surgical robot system meets the adjustment requirement.
Specifically, as shown in fig. 11, after receiving an automatic adjustment instruction from a user, the current state of the surgical robot system is firstly determined whether the current state is suitable for performing the adjustment movement of the mechanical arm and the supporting device, if yes, then the automatic adjustment movement of the supporting device and the mechanical arm is performed, and if no, the automatic adjustment process is ended. Therefore, the invention can effectively ensure the operation safety in the automatic adjustment process by carrying out the automatic adjustment movement of the supporting device and the mechanical arm on the premise that the current state of the operation robot system meets the adjustment requirement. Specifically, it may be determined whether the current state of the surgical robot system meets the adjustment requirement by performing a number of safety checks including, but not limited to, checking whether each of the mechanical arms is in a position maintaining state (i.e., whether the mechanical arms are in a stationary state), whether the position of the instrument mounted on each of the mechanical arms has been locked, whether the position of the instrument mounted on each of the mechanical arms is appropriate, for example, when any of the mechanical arms is not in a position maintaining state, the position of the instrument mounted on each of the mechanical arms is not locked, the position of the instrument mounted on each of the mechanical arms is inappropriate, and the like, then it is determined that the current state of the surgical robot system does not meet the adjustment requirement. When each mechanical arm is in a position maintaining state, the position of the instrument mounted on each mechanical arm is locked, and the position of the instrument mounted on each mechanical arm is proper, the current state of the surgical robot system is judged to meet the adjustment requirement.
Further, as shown in fig. 11, when the computer program is executed by the processor, the following steps are also implemented:
And displaying the real-time adjustment movement process of the mechanical arm and the supporting device.
Therefore, the real-time adjustment movement process of the mechanical arm and the supporting device is displayed in the automatic adjustment process, so that a user can conveniently observe the adjustment movement process of the mechanical arm and the supporting device.
With continued reference to fig. 12, a schematic diagram of a monitoring flow of an adjustment state according to an embodiment of the invention is shown. As shown in fig. 12, the computer program, when executed by the processor, further performs the steps of:
And monitoring the real-time adjustment movement process of each mechanical arm and each supporting device so as to judge whether abnormal conditions occur.
Specifically, as shown in fig. 12, during the automatic adjustment process of the mechanical arms and the supporting device, the real-time adjustment motion process of each mechanical arm and the supporting device is monitored, and whether an abnormal condition exists in the adjustment process is judged, so that the automatic adjustment process can be immediately stopped when the abnormal condition occurs in the adjustment process, and the safety of the patient is protected. For example, if the patient is determined to be under excessive stress at the punching site, the patient is determined to be abnormal, and if the displacement of the robot arm stationary point in the support device coordinate system is large, the patient is also determined to be abnormal.
Further, as shown in fig. 12, when the computer program is executed by the processor, the following steps are also implemented:
Judging whether the mechanical arm and/or the supporting device move to a target pose;
if yes, the target pose is saved.
Therefore, in the automatic adjustment process of the mechanical arm and the supporting device, whether the mechanical arm and/or the supporting device moves to the target pose is judged, so that when the mechanical arm and/or the supporting device moves to the target pose, the target pose can be stored, and the automatic adjustment process is finished.
The present invention further provides an adjustment system, please refer to fig. 13, which schematically illustrates a frame structure of the adjustment system provided in an embodiment of the present invention, as shown in fig. 13, where the adjustment system includes a surgical robot system, the surgical robot system includes a robot 200, the robot 200 includes a robot base 210, and at least one mechanical arm 220 is mounted on the robot base 210.
With continued reference to fig. 13, the adjustment system further includes a positioning device 400, a support device 100, and a controller 300, where the robot 200, the positioning device 400, and the support device 100 are all communicatively connected to the controller 300. The controller 300 includes a processor and readable storage media as described above. The supporting device 100 has multiple degrees of freedom (the supporting device has multiple joints to realize motion with multiple degrees of freedom such as movement, pitching, swaying, etc.), and the specific structure of the supporting device can refer to a multi-degree-of-freedom sickbed in the prior art, which is not described herein. The supporting device 100 includes a supporting device base 110 and a supporting body 120 mounted on the supporting device base 110. The support device 100 is used to support the surgical object (e.g., a surgical object), that is, the surgical object may lie or sit on the support device 200 to perform a surgery, and the support device 200 may be a hospital bed or other components capable of supporting the surgical object for performing a surgical operation other than the hospital bed.
In some embodiments, the controller 300 may be provided in combination with any one or more devices in the surgical robotic system, such as at a physician control end 600 of the surgical robotic system, at the robot 100, at a display device 500 described below, etc., in some embodiments, the controller 300 may also be provided at the positioning device 400, in other embodiments, the controller 300 may be provided separately, and the controller 300 may be a specific hardware or software unit, or a combination of hardware and software unit, without limitation to the specific configuration of the controller 300.
With continued reference to fig. 14, a schematic block diagram of a processor in a controller according to an embodiment of the present invention is shown. As shown in fig. 14, the processor specifically includes a target pose acquisition module 310 and a control module 320. The target pose obtaining module 310 is configured to obtain a target pose of the mechanical arm 220 or a target pose of the supporting device 100, and the control module 320 is configured to control the mechanical arm 220 to perform an adjustment motion and control the supporting device 100 to perform a corresponding motion along with the mechanical arm 220 according to the target pose of the mechanical arm 220, or control the supporting device 100 to perform an adjustment motion and control the mechanical arm 220 to perform a corresponding motion along with the supporting device 100 according to the target pose of the supporting device 100.
As shown in fig. 14, the target pose obtaining module 310 further includes a memory unit 310 and a resolving unit 312, where the memory unit 310 is configured to obtain, according to a correspondence between a pre-stored target pose and a surgery type, the target pose of the mechanical arm 220 or the target pose of the supporting device 100, for example, the memory unit is configured to store a preset mechanical arm and a sickbed configuration, and store the preset configuration, so as to enable an automatic recovery in surgery to the preset configuration, and the preset configuration can avoid collision in surgery of the mechanical arm and/or has an optimal operation space, and the resolving unit 312 is configured to obtain, according to a preset target function, the target pose of the mechanical arm 220 or the target pose of the supporting device 100.
The memory unit 310 is preset with a plurality of different surgical configurations (i.e., target pose) of the mechanical arm 220 and the supporting device 100, such as a kidney-type surgical configuration, a prostate-type surgical configuration, etc., and the calculating unit 312 may calculate an optimal configuration of the mechanical arm 220 or the supporting device 100 according to a state of the surgical robot system and a specific requirement of a user (e.g., a medical staff) through an optimization algorithm. During actual operation, a user (e.g., a medical staff) may select a corresponding operation type to enter a recovery mode through a physical key or a virtual key, and the control module 320 may control the mechanical arm 220 or the supporting device 100 to automatically move to the configuration position according to the configuration of the mechanical arm 220 or the supporting device 100 acquired by the memory unit 310. The user (e.g., a medical staff) may also select to enter the setting mode through a physical key or a virtual key, at this time, the control module 320 may control the mechanical arm 220 or the supporting device 100 to automatically move to the optimal configuration according to the optimal configuration of the mechanical arm 220 or the supporting device 100 calculated by the calculating unit 312. After determining the optimal configuration of the robotic arm 220 or the support device 100, a user (e.g., a medical staff member) may save the information related to the optimal configuration through the memory unit 310, so that the user (e.g., the medical staff member) may directly control the robotic arm 220 or the support device 100 to return to the saved optimal configuration through the controller 300 during a subsequent operation.
It should be noted that, in other embodiments, the target pose acquisition module 310 may include only the memory unit 310, and in other embodiments, the target pose acquisition module 310 may include only the resolving unit 312, as will be appreciated by those skilled in the art.
As shown in fig. 14, the control module 320 includes a trajectory planning unit 321 and an adjusting unit 322. The track planning unit 321 is configured to plan a motion track of the mechanical arm 220 according to a target pose and a current pose of the mechanical arm 220, or plan a motion track of the supporting device 100 according to the target pose and the current pose of the supporting device 100, and the adjusting unit 322 is configured to control each mechanical arm 220 to perform an adjusting motion according to the motion track according to the planned motion track of the mechanical arm 220, and control the supporting device 100 to perform a corresponding motion along the motion track of the mechanical arm 220, or control the supporting device 100 to perform an adjusting motion according to the motion track of the supporting device 100 according to the planned motion track of the supporting device 100, and control the mechanical arm 220 to perform a corresponding motion along the motion track of the supporting device 100.
Further, the trajectory planning unit 321 is configured to plan a motion trajectory of each joint of the mechanical arm 220 according to the target position and the current position of each joint of the mechanical arm 220, or plan a motion trajectory of each joint of the supporting device 100 according to the target position and the current position of each joint of the supporting device 100.
Further, the trajectory planning unit 321 is configured to obtain a motion trajectory of the mechanical arm 220 (a motion trajectory of each joint of the mechanical arm 220) by using an interpolation algorithm according to a target pose and a current pose of the mechanical arm 220 (a target position and a current position of each joint of the mechanical arm 220), or obtain a motion trajectory of the supporting device 100 (a motion trajectory of each joint of the supporting device 100) by using an interpolation algorithm according to a target pose and a current pose of the supporting device 100 (a target position and a current position of each joint of the supporting device 100).
Referring to fig. 15, a schematic diagram of a measurement principle of a positioning device according to an embodiment of the invention is schematically shown. As shown in fig. 15, the positioning device 400 is a binocular camera, that is, in the present embodiment, the positioning device 400 obtains a mapping relationship between a robot coordinate system (X2, Y2, Z2) and a world coordinate system (X0, Y0, Z0) and a real-time mapping relationship between a supporting device coordinate system (X1, Y1, Z1) and the world coordinate system (X0, Y0, Z0) based on a binocular vision measurement principle. The support body 120 of the support device 100 is provided with a plurality of first markers 130, further, in order to improve measurement accuracy, the support device base 110 may also be provided with a plurality of first markers 130, the robot base 210 is provided with a plurality of second markers 240, the images of the plurality of first markers 130 are acquired by the binocular camera, coordinates of the plurality of first markers 130 in the world coordinate system (X0, Y0, Z0) may be acquired, and then, according to a mapping relationship between the plurality of first markers 130 and the support device coordinate system (X1, Y1, Z1), a mapping relationship between the support device coordinate system (X1, Y1, Z1) and the world coordinate system (X0, Y0, Z0) may be acquired. Similarly, the images of the plurality of second markers 240 are acquired by the binocular camera, so that coordinates of the plurality of second markers 240 in the world coordinate system (X0, Y0, Z0) can be acquired, and then, according to a mapping relationship between the plurality of second markers 240 and the robot coordinate system (X2, Y2, Z2), a mapping relationship between the robot coordinate system (X2, Y2, Z2) and the world coordinate system (X0, Y0, Z0) can be acquired.
It should be noted that, in other embodiments, the positioning device 400 may also obtain a mapping relationship between the robot coordinate system (X2, Y2, Z2) and the world coordinate system (X0, Y0, Z0) and a real-time mapping relationship between the support device coordinate system (X1, Y1, Z1) and the world coordinate system (X0, Y0, Z0) based on a common position measurement method such as a monocular vision measurement method, an optical tracking measurement method, or an electromagnetic measurement method, which is not limited in the present invention.
Further, after the adjustment movement of the mechanical arm 220 and the supporting device 100 is completed, the system stores the adjusted pose of the mechanical arm 220 and the adjusted pose of the supporting device 100, and the whole automatic adjustment process is finished. By this, by storing the adjusted pose of the robot arm 220 and the adjusted pose of the supporting device 100, the robot arm 220 and the supporting device 100 can be directly restored to the stored pose in the subsequent operation.
As shown in fig. 14, the processor further includes a state determination module 330, where the state determination module 330 is configured to determine whether the current state of the surgical robot system is suitable for performing the adjustment movement of the robotic arm 220 and the support device 100. Specifically, when the user (for example, a medical staff) sends the automatic adjustment instruction to the controller 300 through a physical key on the surgical robot system or a virtual key on the interactive interface (i.e., after the controller 300 receives the user instruction), in order to ensure the safety in the automatic adjustment process, before entering the automatic adjustment process, the state determination module 330 performs a plurality of safety detections to determine whether the current state of the surgical robot system is suitable for performing the adjustment movements of the mechanical arm 220 and the supporting device 100, and if the determination result is yes, performs the automatic adjustment movements of the supporting device 100 and the mechanical arm 220. If the judgment result is negative, the automatic adjustment request of the user (such as medical staff) is forbidden, and the automatic adjustment flow is directly ended. To facilitate timely awareness by a user (e.g., healthcare personnel), the surgical robotic system may provide the user (e.g., healthcare personnel) with several pieces of explanatory information, or feedback in the form of audible and visual alarms, under the control of the controller 300.
As shown in fig. 14, the processor further includes a monitoring module 340, where the monitoring module 340 is configured to monitor the adjustment motion of each of the robotic arms 220 and the support device 100.
As shown in fig. 13, the adjustment system further includes a display device 500 communicatively connected to the controller 300, where the display device 500 is configured to display the real-time adjustment movement process of the robot arm 220 and the support device 100. Thus, the display device 500 can display the adjustment movement process of the manipulator 220 and the support device 100 in real time, wherein the display content is shown as a right box in fig. 16, and a user (for example, a medical staff) can observe an image photographed by an endoscope mounted on the manipulator 220 through the display device 500, and the display content is shown as a left box in fig. 16, so as to prevent the instrument 230 mounted on the manipulator 220 from damaging the tissue of a patient.
It should be noted that any combination of one or more computer readable media may be employed as the readable storage media of embodiments of the present invention. The readable medium may be a computer readable signal medium or a computer readable storage medium. The computer readable storage medium can be, for example, but not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or a combination of any of the foregoing. More specific examples (a non-exhaustive list) of the computer-readable storage medium include an electrical connection having one or more wires, a portable computer hard disk, a Random Access Memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disc read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing. In the context of this document, a computer readable storage medium may be any tangible medium that can contain, or store a program for use by or in connection with an instruction execution system, apparatus, or device.
The computer readable signal medium may include a propagated data signal with computer readable program code embodied therein, either in baseband or as part of a carrier wave. Such a propagated data signal may take any of a variety of forms, including, but not limited to, electro-magnetic, optical, or any suitable combination of the foregoing. A computer readable signal medium may also be any computer readable medium that is not a computer readable storage medium and that can communicate, propagate, or transport a program for use by or in connection with an instruction execution system, apparatus, or device.
Computer program code for carrying out operations of the present invention may be written in any combination of one or more programming languages, including an object oriented programming language such as Java, smalltalk, C ++ and conventional procedural programming languages, such as the "C" programming language or similar programming languages. The program code may execute entirely on the user's computer, partly on the user's computer, as a stand-alone software package, partly on the user's computer and partly on a remote computer or entirely on the remote computer or server. In the case of a remote computer, the remote computer may be connected to the user's computer through any kind of network, including a Local Area Network (LAN) or a Wide Area Network (WAN), or may be connected to an external computer (for example, through the Internet using an Internet service provider).
In summary, compared with the prior art, the readable storage medium, the surgical robot system and the adjusting system have the advantages that the target pose of the mechanical arm of the robot is firstly obtained, then the mechanical arm is adjusted to move so as to adjust the pose of the mechanical arm to the target pose, and in the adjusting process of the mechanical arm, the supporting device is controlled to move correspondingly along with the mechanical arm, or the target pose of the supporting device is firstly obtained, then the supporting device is adjusted to move so as to adjust the pose of the supporting device to the target pose, and in the adjusting process of the supporting device, the mechanical arm of the robot is controlled to move correspondingly along with the supporting device. Therefore, the invention can realize the adjustment of the body position (namely the position of the supporting device) of the patient and the position of the mechanical arm under the condition of not withdrawing the instrument, thereby completing the operation more efficiently and safely, reducing the requirements on the position of punching before operation and the positioning of equipment, effectively reducing the preparation time before operation and effectively avoiding the collision probability of the mechanical arm.
It should be noted that the apparatus and methods disclosed in the embodiments herein may be implemented in other ways. The apparatus embodiments described above are merely illustrative, for example, flow diagrams and block diagrams in the figures illustrate the architecture, functionality, and operation of possible implementations of apparatus, methods and computer program products according to various embodiments herein. In this regard, each block in the flowchart or block diagrams may represent a module, segment, or portion of code, which comprises one or more executable instructions for implementing the specified logical function(s). It should also be noted that in some alternative implementations, the functions noted in the block may occur out of the order noted in the figures. For example, two blocks shown in succession may, in fact, be executed substantially concurrently, or the blocks may sometimes be executed in the reverse order, depending upon the functionality involved. It will also be noted that each block of the block diagrams and/or flowchart illustration, and combinations of blocks in the block diagrams and/or flowchart illustration, can be implemented by special purpose hardware-based systems which perform the specified functions or acts, or combinations of special purpose hardware and computer instructions.
In addition, the functional modules in the embodiments herein may be integrated together to form a single part, or the modules may exist alone, or two or more modules may be integrated to form a single part.
The above description is only illustrative of the preferred embodiments of the present invention and is not intended to limit the scope of the present invention, and any alterations and modifications made by those skilled in the art based on the above disclosure shall fall within the scope of the present invention. It will be apparent to those skilled in the art that various modifications and variations can be made to the present invention without departing from the spirit or scope of the invention. Thus, the present invention is intended to include such modifications and alterations insofar as they come within the scope of the invention or the equivalents thereof.