WO2022258054A1 - 重建器械术野中心的控制方法、系统和存储介质 - Google Patents

重建器械术野中心的控制方法、系统和存储介质 Download PDF

Info

Publication number
WO2022258054A1
WO2022258054A1 PCT/CN2022/098189 CN2022098189W WO2022258054A1 WO 2022258054 A1 WO2022258054 A1 WO 2022258054A1 CN 2022098189 W CN2022098189 W CN 2022098189W WO 2022258054 A1 WO2022258054 A1 WO 2022258054A1
Authority
WO
WIPO (PCT)
Prior art keywords
platform
pose
static
coordinate system
transformation
Prior art date
Application number
PCT/CN2022/098189
Other languages
English (en)
French (fr)
Inventor
黄善灯
柏龙
陈晓红
潘鲁锋
Original Assignee
诺创智能医疗科技(杭州)有限公司
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 诺创智能医疗科技(杭州)有限公司 filed Critical 诺创智能医疗科技(杭州)有限公司
Priority to EP22819651.5A priority Critical patent/EP4353180A1/en
Publication of WO2022258054A1 publication Critical patent/WO2022258054A1/zh
Priority to US18/535,498 priority patent/US20240123609A1/en

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • 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/30—Surgical robots
    • 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/30—Surgical robots
    • A61B34/37—Master-slave robots
    • 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
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J17/00—Joints
    • B25J17/02—Wrist joints
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J18/00—Arms
    • B25J18/02—Arms extensible
    • B25J18/025—Arms extensible telescopic
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/003—Programme-controlled manipulators having parallel kinematics
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/06—Programme-controlled manipulators characterised by multi-articulated arms
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1615—Programme controls characterised by special kind of manipulator, e.g. planar, scara, gantry, cantilever, space, closed chain, passive/active joints and tendon driven manipulators
    • B25J9/162—Mobile manipulator, movable base with manipulator arm mounted on it

Definitions

  • the present disclosure relates to the field of robot technology, and in particular to a control method, system, device and storage medium for reconstructing the surgical field center of an instrument.
  • the robot Before the application operation, the robot needs to be positioned so that the end of the instrument can reach the corresponding operating space.
  • the positioning first depends on the movement of the serial manipulator to make the end of the instrument reach the operating space, and then through the control of the parallel platform. The end of the instrument reaches the middle of the operating space to achieve precise positioning.
  • the parallel platform When transferring the instrument to another remote location, if the instrument is directly moved to the target point through the active arm, the parallel platform may already be at the edge of the working space at this time, so it is necessary to keep the position of the instrument unchanged. Adjust the parallel platform to zero position.
  • the active arm of the parallel robot has a small range of motion. After the positioning is completed, the parallel platform is usually not at the zero position, which may cause the parallel platform to fail to cover the application’s operating space in the motion space, so that the parallel platform is stuck at the limit position and cannot complete the operation. . Due to the limited freedom of movement of the passive arm, attitude return to zero cannot be fully realized only through the movement of the passive arm.
  • Embodiments of the present disclosure provide a control method, system, device and readable storage medium for reconstructing the center of an operating field of an instrument.
  • an embodiment of the present disclosure provides a control method for reconstructing the center of the surgical field of an instrument, including:
  • the current pose of the moving platform in the mechanical coordinate system where the mechanical coordinate system is the global coordinate system of the robotic arm where the moving platform is located;
  • the target pose of the static platform in the mechanical coordinate system is obtained after the center of the surgical field is reconstructed.
  • the center of the pose is on the central axis of the current pose of the moving platform, and the static platform in the target pose is parallel to the moving platform;
  • the torsion angle between the moving platform and the static platform on the central axis is obtained, and the moving platform is rotated and transformed according to the torsion angle to make the parallel platform return to zero.
  • an embodiment of the present disclosure provides a method for controlling a robotic arm, wherein the robotic arm includes a parallel platform, and the parallel platform includes a dynamic platform, a static platform, and a connecting platform between the dynamic platform and the static platform.
  • the telescopic element includes the following steps: obtaining the current pose of the moving platform in a mechanical coordinate system, wherein the mechanical coordinate system is the global coordinate system of the mechanical arm where the moving platform is located; according to the the current pose of the moving platform, obtain the target pose of the static platform in the mechanical coordinate system, and perform pose transformation.
  • an embodiment of the present disclosure provides a method for controlling a robotic arm, wherein the method includes the following steps: acquiring the current pose of the moving platform in a mechanical coordinate system, wherein the mechanical coordinate system is the The global coordinate system of the mechanical arm where the moving platform is located; the target pose of the static platform under the mechanical coordinate system is obtained according to the current pose of the moving platform, wherein the moving platform and the static platform are the mechanical arms Parallel platforms connected by telescopic elements; perform pose transformation on the static platform according to the current pose of the moving platform and the target pose of the static platform.
  • an embodiment of the present disclosure provides a control system for reconstructing the surgical field center of an instrument, including:
  • the first acquisition module is used to acquire the current pose of the moving platform in the mechanical coordinate system after reconstructing the surgical field center of the instrument, wherein the mechanical coordinate system is the global coordinate system of the mechanical arm where the moving platform is located;
  • the second acquisition module is used to obtain the target pose of the static platform in the mechanical coordinate system after reconstructing the center of the surgical field of the instrument according to the current pose of the moving platform, wherein the moving platform and the static platform are connected by telescopic elements in the robotic arm Parallel platforms, the center of the target pose of the static platform is on the central axis of the current pose of the moving platform, and the static platform in the target pose is parallel to the moving platform;
  • the first transformation module is used to perform pose transformation on the static platform according to the current pose of the moving platform and the target pose of the static platform;
  • the second transformation module is used to obtain the twist angle between the moving platform and the static platform on the central axis according to the current pose of the moving platform and the target pose of the static platform, and perform rotation transformation on the moving platform according to the twist angle, so that the parallel The platform returns to zero.
  • an embodiment of the present disclosure provides a control system for a robotic arm, wherein the robotic arm includes a parallel platform, and the parallel platform includes a dynamic platform, a static platform, and a
  • the telescopic element the system includes: a first acquisition module, configured to acquire the current pose of the moving platform in a mechanical coordinate system, wherein the mechanical coordinate system is the global coordinates of the mechanical arm where the moving platform is located system; the second acquisition module is used to acquire the target pose of the static platform in the mechanical coordinate system according to the current pose of the moving platform; and the first transformation module is used to obtain the target pose of the static platform in the mechanical coordinate system; Perform pose transformation on the static platform between the current pose and the target pose of the static platform.
  • an embodiment of the present disclosure provides a robotic arm assembly, the robotic arm assembly includes: a robotic arm; and a control device, the control device is used for: controlling the surgical field center of the reconstruction instrument according to the above first aspect
  • the method performs pose transformation on the static platform on the mechanical arm, performs rotation transformation on the dynamic platform on the mechanical arm, or performs pose transformation.
  • an embodiment of the present disclosure provides a surgical robot, which includes: a base; and the mechanical arm assembly as described in the sixth aspect.
  • an embodiment of the present disclosure provides a readable storage medium, on which an executable program is stored, and when the executable program is executed by a processor, the method for controlling the surgical field center of the reconstructed instrument as described in the first aspect or The control method of the mechanical arm as in the second aspect above.
  • an embodiment of the present disclosure provides a computer program product, which includes computer instructions, wherein, when the computer instructions are executed by a processor, the method for controlling the surgical field center of the reconstructed instrument as described in the first aspect above or as described above The control method of the mechanical arm of the second aspect.
  • the control method, system, device, and readable storage medium provide the current pose of the moving platform in the mechanical coordinate system after the center of the surgical field of the reconstructed instrument is obtained.
  • the target pose of the static platform in the mechanical coordinate system is obtained, and the pose of the static platform is transformed according to the current pose of the moving platform and the target pose of the static platform; according to The current pose of the moving platform and the target pose of the static platform obtain the torsion angle between the moving platform and the static platform on the central axis, and rotate and transform the moving platform according to the torsion angle so that the parallel platform returns to zero, which solves the problem of The problem that the parallel robot in the technology is difficult to achieve attitude return to zero improves the control accuracy of the center of the surgical field of the reconstructed instrument and ensures the kinematic performance of the parallel platform.
  • Fig. 1 is a schematic flowchart of a method for controlling the center of an operation field of a reconstruction instrument according to an embodiment of the present disclosure
  • Fig. 2 is an application schematic diagram of a method for controlling the center of an operation field of a reconstruction instrument according to an embodiment of the present disclosure
  • FIG. 3 is a schematic diagram of a joint of a robotic arm according to an embodiment of the present disclosure
  • Fig. 4 is a schematic flowchart of a first example of the steps of performing pose transformation on a static platform according to an embodiment of the present disclosure
  • Fig. 5 is a schematic flowchart of the steps of transforming the attitude parameters of the joints of the passive arm according to an embodiment of the present disclosure
  • Fig. 6 is a schematic flowchart of the steps of transforming the driving parameters of the second joint according to an embodiment of the present disclosure
  • Fig. 7 is a schematic flowchart of a second example of the step of performing pose transformation on a static platform according to an embodiment of the present disclosure
  • Fig. 8 is a schematic flowchart of a third example of the step of performing pose transformation on a static platform according to an embodiment of the present disclosure
  • FIG. 9 is a schematic diagram of pose transformation of a static platform in a parallel platform according to an embodiment of the present disclosure.
  • Fig. 10 is a schematic diagram of attitude transformation of a static platform in a parallel platform according to an embodiment of the present disclosure
  • Fig. 11 is a schematic flowchart of the steps of performing position transformation and attitude transformation on a static platform according to an embodiment of the present disclosure
  • FIG. 12 is a schematic diagram of a system default initial position of a parallel platform according to an embodiment of the present disclosure
  • Fig. 13 is a schematic flowchart of a first example of the steps of performing rotational transformation on a moving platform according to an embodiment of the present disclosure
  • Fig. 14 is a schematic flowchart of a second example of the steps of performing rotation transformation on the moving platform according to an embodiment of the present disclosure
  • Fig. 15 is a schematic flowchart of the steps of obtaining the coordinates of the telecentric fixed point in the static coordinate system after reconstruction of the surgical field according to an embodiment of the present disclosure
  • 16 is a schematic flowchart of a method for controlling a robotic arm according to an embodiment of the present disclosure
  • Fig. 17 is a schematic flowchart of the steps of performing pose transformation on a static platform in a method for controlling a robotic arm according to an embodiment of the present disclosure
  • Fig. 18 is a schematic flowchart of the steps of transforming the attitude parameters of the joints of the passive arm in the method for controlling the robotic arm according to an embodiment of the present disclosure
  • Fig. 19 is a structural block diagram of the control system of the surgical field center of the reconstruction instrument according to an embodiment of the present disclosure
  • Fig. 20 is a structural block diagram of a control system of a robotic arm according to an embodiment of the present disclosure
  • Fig. 21 is a structural block diagram of a control device in the surgical field center of a reconstruction instrument according to an embodiment of the present disclosure
  • FIG. 22 is a structural block diagram of a robotic arm assembly according to an embodiment of the present disclosure.
  • FIG. 23 is a structural block diagram of a surgical robot according to an embodiment of the present disclosure.
  • Words such as “connected”, “connected”, “coupled” and the like involved in the present disclosure are not limited to physical or mechanical connections, but may include electrical connections, no matter direct or indirect.
  • the "plurality” referred to in the present disclosure means two or more.
  • “And/or” describes the association relationship of associated objects, indicating that there may be three types of relationships. For example, “A and/or B” may indicate: A exists alone, A and B exist simultaneously, and B exists independently.
  • the character “/” generally indicates that the contextual objects are an “or” relationship.
  • the terms “first”, “second”, and “third” involved in the present disclosure are only used to distinguish similar objects, and do not represent a specific ordering of objects.
  • FIG. 1 is a schematic flow chart of a control method for reconstructing the surgical field center of an instrument according to an embodiment of the present disclosure. As shown in Fig. 1 , the process includes the following steps:
  • Step S110 After obtaining the center of the surgical field of the reconstructed instrument, the current pose of the moving platform in the mechanical coordinate system, wherein the mechanical coordinate system is the global coordinate system of the mechanical arm where the moving platform is located;
  • the instrument operation field center refers to the position of the instrument in the center of the surgical field of view
  • the reconstructed instrument operation field center refers to the non-target area of the operation field center at the end of the mechanical arm of the parallel robot.
  • the center of the surgical field is re-determined so that the center of the surgical field is in the target area; in the process of reconstructing the center of the surgical field of the instrument, the transfer of the center of the surgical field is generally realized by moving the moving platform on the parallel platform of the parallel robot, but this The dynamic platform and the static platform on the parallel platform are no longer at the zero position; the global coordinate system of the manipulator itself is the mechanical coordinate system applicable to all parts of the manipulator, and the poses of the dynamic platform and the static platform are mapped to the mechanical coordinate system , which is beneficial to the calculation of pose transformation between the two.
  • the mechanical coordinate system may be the global coordinate system in which the entire mechanical arm in FIG. 3 is located, and the origin of the global coordinate system may be the center of the base of the mechanical arm.
  • Step S120 Obtain the target pose of the static platform in the mechanical coordinate system after obtaining the center of the surgical field of the reconstructed instrument according to the current pose of the moving platform.
  • the center of the target pose of the platform is on the central axis of the current pose of the moving platform, and the static platform in the target pose is parallel to the moving platform;
  • the parallel platform is an important part of the manipulator, including the dynamic platform, the telescopic element and the static platform. Since the relative poses of the static platform and the dynamic platform at zero position are determined, the target pose of the static platform is The center is on the central axis of the current pose of the moving platform, and the static platform in the target pose is parallel to the moving platform. The target of the static platform in the mechanical coordinate system can be determined by using the current pose of the moving platform after reconstructing the center of the instrument field. pose.
  • Step S130 perform pose transformation on the static platform according to the current pose of the moving platform and the target pose of the static platform;
  • the current pose of the moving platform and the target pose of the static platform are used as the target of the transformation, and the pose transformation is performed on the static platform, so that the static platform moves to the target pose.
  • the moving platform is The current pose in the machine coordinate system remains unchanged.
  • Step S140 Obtain the torsion angle between the moving platform and the static platform on the central axis according to the current pose of the moving platform and the target pose of the static platform, and perform rotation transformation on the moving platform according to the twisting angle, so that the parallel platform returns to zero ;
  • the parallel platform In this step, after the pose transformation of the static platform, the parallel platform cannot completely return to the zero position, and there may be a twist in the direction of the central axis of the moving platform between the moving platform and the static platform. Due to the limitation of the degree of freedom of the arm, it is impossible to return the parallel platform to the zero position by adjusting the posture of the static platform. The axis rotates, but this will not violate the principle of telecentric fixed point, nor will it change the center of the surgical field of the instrument, and the reconstruction of the center of the surgical field can be effectively completed.
  • the current pose of the moving platform in the mechanical coordinate system after obtaining the center of the surgical field of the reconstructed instrument according to the current pose of the moving platform, the position of the static platform in the mechanical coordinate system According to the target pose of the moving platform and the target pose of the static platform, the pose transformation of the static platform is performed; according to the current pose of the moving platform and the target pose of the static platform, the distance between the moving platform and the static platform is obtained.
  • the moving platform is rotated and transformed according to the twist angle, so that the parallel platform can return to zero position, which solves the problem that the parallel robot in the related technology is difficult to achieve the attitude return to zero, and improves the surgical field of reconstruction instruments.
  • the control accuracy of the center ensures the motion performance of the parallel platform.
  • the method for controlling the center of the surgical field of the reconstruction instrument disclosed in the present disclosure can be applied to various application scenarios.
  • the front assistant establishes the telecentric fixed point by moving the joints of the passive arm, and at the same time establishes the center of the surgical field of the instrument so that the initial position of the end point of the instrument is at the center of the surgical field. center. Since the instrument is not inserted into the object to be inspected in this process, but is roughly determined by the front-end assistant's visual inspection, there may be a large deviation from the ideal center of the instrument's surgical field.
  • the solution of the present disclosure can be used to reconstruct the center of the surgical field of the instrument without changing the fixed point of the far center, so as to eliminate the deviation and obtain the ideal movement of the instrument space.
  • the working space of the parallel platform is relatively small, and some scenarios may require a larger activity space.
  • the center of the instrument operation field can be reconstructed through the scheme of the present disclosure, and the working space of the parallel platform can be moved without changing the fixed point of the telecentricity , and indirectly obtain a larger activity space.
  • performing pose transformation on the static platform according to the current pose of the moving platform and the target pose of the static platform includes the following steps:
  • Step S410 According to the current pose of the moving platform and the target pose of the static platform, obtain the first conversion matrix between the static coordinate system and the moving coordinate system when the reconstruction is performed and returns to the zero state;
  • the static coordinate system is the coordinate of the static platform system,
  • the moving coordinate system is the coordinate system where the moving platform is located;
  • Step S420 Obtain the second transformation matrix between the static coordinate system and the dynamic coordinate system when the reconstruction has not returned to the zero position state; and the third transformation matrix between the mechanical coordinate system and the static coordinate system before reconstruction;
  • Step S430 Obtain a fourth transformation matrix between the reconstructed mechanical coordinate system and the static coordinate system according to the first transformation matrix, the second transformation matrix and the third transformation matrix;
  • Step S440 Obtain a fifth transformation matrix between the joint coordinate systems of the passive arm in the robotic arm according to the fourth transformation matrix, and transform the attitude parameters of the joints of the passive arm according to the fifth transformation matrix, wherein the passive arm is connected to the static platform.
  • transformation matrix Before and after reconstruction of the surgical field center of the instrument, there is a transformation relationship among the dynamic coordinate system, the static coordinate system and the mechanical coordinate system, which can be described by a transformation matrix.
  • Four transformation matrices are used here: the first transformation matrix between the static coordinate system and the dynamic coordinate system when it is reconstructed and returned to the zero position state; the first transformation matrix between the static coordinate system and the dynamic coordinate system The second transformation matrix; the third transformation matrix between the mechanical coordinate system and the static coordinate system before reconstruction; the fourth transformation matrix between the mechanical coordinate system and the static coordinate system after reconstruction.
  • the first transformation matrix can be obtained according to the current pose of the moving platform and the target pose of the static platform
  • the second transformation matrix can be obtained according to the transformation relationship between the moving platform and the static platform
  • the third transformation matrix can be obtained according to the passive arm of the manipulator
  • the joint information transformation of the moving platform is obtained, because the pose of the moving platform remains unchanged during the transformation process, the product of the fourth transformation matrix and the first transformation matrix is the same as the product of the third transformation matrix and the second transformation matrix, using this equivalence relationship
  • the fourth transformation matrix can be obtained, which reflects the transformation relationship from the passive arm to the parallel platform in the manipulator, thus the fifth transformation matrix between the joint coordinate systems of the passive arm in the manipulator can be obtained, and the fifth transformation matrix
  • the matrix transforms the attitude parameters of the joints of the passive arm, and realizes the attitude transformation of the static platform in the parallel platform through the attitude transformation of the passive arm.
  • the static coordinate system is the coordinate system where the static platform is located.
  • the Xs axis and the Ys axis are parallel to the plane of the static platform, and the Zs axis is perpendicular to the static platform plane.
  • Platform plane after reconstructing the operation field, take the center S' of the static platform as the origin, the X's axis and Y's axis are parallel to the plane of the static platform, and the Z's axis is perpendicular to the plane of the static platform;
  • the dynamic coordinate system is the coordinate system where the moving platform is located.
  • the passive arm 10 includes a first joint 11, a second joint 12, and a third joint 13 connected in series, wherein the posture parameters of the first joint and the third joint remain unchanged, and the first joint
  • the three joints are connected to the static platform 21 of the parallel platform 20, and the dynamic platform 22 and the static platform 21 are connected through telescopic elements 23;
  • transforming the posture parameters of the joints of the passive arm according to the fifth transformation matrix includes the following steps:
  • Step S510 Obtain the sixth transformation matrix from the mechanical coordinate system to the coordinate system of the first joint, and the seventh transformation matrix from the coordinate system of the second joint to the coordinate system of the third joint;
  • Step S520 Obtain an eighth transformation matrix between the first joint and the second joint according to the fifth transformation matrix, the sixth transformation matrix and the seventh transformation matrix;
  • Step S530 Transform the posture parameters of the second joint according to the eighth transformation matrix.
  • the passive arm includes a plurality of joints in series.
  • the attitude parameters of the joints of the passive arm are transformed, the attitude parameters of some of the joints remain unchanged, and only the attitude parameters of the other part of the joints are changed. Due to the first The attitude parameters of the joint and the third joint remain unchanged, the sixth transformation matrix from the mechanical coordinate system to the coordinate system of the first joint, and the seventh transformation matrix between the coordinate system of the second joint and the coordinate system of the third joint are determined Yes, combined with the fifth transformation matrix of the transformation target, the eighth transformation matrix for the posture parameters of the second joint can be obtained, and the posture parameters of the second joint are transformed based on the eighth transformation matrix, so that a small number of motion joints can be used, Reduce the amount of calculation of the attitude parameters of the joints, and realize the adjustment of the passive arm as soon as possible.
  • first joint, the second joint and the third joint may include one or more joints, and since the third joint is connected to the static platform, when the operation is completed and the parallel platform is exited, a sufficient range of motion of the third joint is guaranteed.
  • the second joint includes a first rotating pair, a first moving pair, a second rotating pair, a second moving pair and a third rotating pair connected in sequence, and the first rotating pair is connected to the first rotating pair.
  • the joint is rotationally connected, and the third rotating pair is rotationally connected with the third joint;
  • said transforming the driving parameters of the second joint according to the eighth transformation matrix includes the following steps:
  • Step S610 Obtain the transformation relationship of the driving parameters from the first joint to the third rotating pair, and obtain the first rotating pair, the first rotating pair according to the eighth transformation matrix and the driving parameter transformation relationship. driving parameters of the moving pair, the second rotating pair, the second moving pair and the third rotating pair.
  • the second joint may include a plurality of joints, such as a first rotating pair, a first moving pair, a second rotating pair, a second moving pair and a third rotating pair connected in sequence, and the first rotating pair and all
  • the first joint is rotationally connected
  • the third rotating pair is rotationally connected to the third joint
  • the multiple joints included in the second joint can make the passive arm have a larger movement space, due to the joints in the second joint Connection relationship, there is a certain transformation relationship between the driving parameters of each joint, combined with the eighth transformation matrix, the specific driving parameters of each joint can be solved specifically, so as to realize the adjustment of the second joint of the passive arm.
  • the driving parameter of the first rotating pair can be the rotation angle ⁇ 5
  • the driving parameter of the first moving pair can be the moving distance d6
  • the driving parameter of the second rotating pair can be the rotating angle ⁇ 7
  • the second The driving parameter of the moving pair may be the moving distance d8, and the driving parameter of the third rotating pair may be the rotation angle ⁇ 9, as shown in FIG. 3 .
  • performing pose transformation on the static platform according to the current pose of the moving platform and the target pose of the static platform includes the following steps:
  • Step S710 Obtain the first expansion and contraction amount of the telescopic element according to the current pose of the moving platform and the target pose of the static platform. The current pose of the platform in the mechanical coordinate system remains unchanged.
  • the solution of the present disclosure can be applied to the Stewart platform, which can be associated with 10 joints in series.
  • the control of the tandem manipulator is usually based on the DH parameters or the improved DH parameters for the transformation of the coordinate system.
  • Two joints that are connected are called adjacent joints.
  • the joint coordinate system transformation of two adjacent joints is usually represented by DH parameters or improved DH parameters.
  • DH parameters as an example, two adjacent joint coordinate systems can overlap each other by rotating ⁇ around the Z axis and translating d, then rotating ⁇ around the X axis and translating a.
  • the above ⁇ , d, ⁇ and a are DH parameters. It can be seen that if the DH parameters are more concise, the conversion of two adjacent joint coordinate systems is simpler.
  • the transformation matrix from the mth joint (including the degree of freedom of the m joint) to the nth joint can be expressed as:
  • T_0_4 represents the transformation matrix between the mechanical coordinate system and the 4th joint coordinate system
  • T_4_9 1 represents the transformation matrix between the 4th joint coordinate system and the 9th joint coordinate system after rebuilding and returning to the zero state
  • T_9_10 represents the 9th joint coordinate system Transformation matrix to the tenth joint coordinate system
  • T_s_end_m_end represents the transformation matrix between the static coordinate system and the dynamic coordinate system of the Stewart platform after reconstruction and returning to the zero state
  • T_machine_s_begin represents the transformation matrix between the machine coordinate system and the static coordinate system before reconstruction
  • T_s_begin_m_end Indicates the conversion matrix between the static coordinate system and the dynamic coordinate system when the Stewart platform does not return to the zero state after reconstruction.
  • the left side of the equation is the transformation matrix containing the parameters to be obtained, and the right side of the equation is the known quantity.
  • T_4_9 0 means the transformation matrix when the DH parameters of joints 5-9 have not been updated, and T_4_9 1 means rebuild and return to zero Conversion matrix between joint 4 coordinate system and joint 9 coordinate system after bit state. Since the DH parameters of the 1st-4th and 10th passive articulated arms do not change throughout the process, the matrix expressions can be obtained accordingly:
  • T_machine_s_begin T_0_4 ⁇ T_4_9 0 ⁇ T_9_10 (6)
  • m oz represents the distance between the dynamic and static coordinate system in the Z-axis direction of the static coordinate system when the state is in zero position.
  • ⁇ x , ⁇ y , ⁇ z be the Euler angles around the x, y, and z axes respectively
  • m x , m y , m z be the coordinate positions of the moving coordinate system in the static coordinate system at any time, then at any time
  • the transformation matrix from the static coordinate system to the dynamic coordinate system is:
  • the conversion matrix from the coordinate system of the fourth joint to the coordinate system of the ninth joint can be obtained as follows:
  • T_4_9 1 [3,3] sin ⁇ 9 (sin ⁇ x cos ⁇ z +cos ⁇ x sin ⁇ y sin ⁇ z )-cos ⁇ 9 cos ⁇ y sin ⁇ z
  • ⁇ 9 is the DH parameter of the ninth joint arm of the series arm
  • ⁇ x is the Euler angle of the moving coordinate system around its own X-axis
  • ⁇ z is the Euler angle of the moving coordinate system around its own Z-axis
  • T_s_m_input is calculated based on the position information transmitted by the main hand and is a known quantity. According to formula (15), the Euler angles of the dynamic coordinate system around its own X and Y axes can be calculated:
  • ⁇ x arcsin(T_s_m_input(3,2))
  • ⁇ y arcsin(T_s_m_input(1,3))
  • the reconstruction process of the surgical field can be divided into “pose transformation stage” and “moving coordinate system return to zero stage”.
  • the "posture transformation stage” is the process of keeping the pose of the moving platform in the mechanical coordinate system, and moving the static platform to a position parallel to the moving platform and collinear with the Z axis;
  • the "moving coordinate system return to zero stage” is The moving platform rotates ⁇ z around its own Z axis to make the Stewart platform return to zero.
  • the conversion matrix of the static coordinate system and the conversion matrix of the dynamic and static coordinate system have the following relationship:
  • T_machine_s_begin ⁇ T_s_begin_m_end T_machine_s_end ⁇ T_s_end_m_end (16)
  • T_machine_s_end T_machine_s_begin ⁇ T_s_begin_m_end ⁇ T_s_end_m_end -1 (17)
  • T_machine_s_end is the conversion matrix from the mechanical coordinate system to the static coordinate system after reconstruction
  • T_s_end_m_end is the zero conversion matrix of the dynamic and static coordinate system
  • performing pose transformation on the static platform according to the current pose of the moving platform and the target pose of the static platform includes the following steps:
  • Step S810 Obtain the original pose of the static platform before reconstruction, interpolate the original pose and the target pose of the static platform, and obtain the intermediate pose of the static platform during the pose transformation process;
  • Step S820 Perform position transformation and attitude transformation on the static platform according to the intermediate pose and the current pose of the moving platform, wherein the position transformation includes circular motion transformation with the far-centered fixed point as the center and linear motion transformation pointing to the center of the circle,
  • the attitude transformation includes the rotation transformation of the static coordinate system around the Z axis of the machine coordinate system and the rotation transformation of the static coordinate system around its own Y axis.
  • the original pose and the target pose of the static platform are interpolated to obtain the intermediate pose of the static platform during the pose transformation process, and the path when the static platform returns to zero can be planned by using the intermediate pose.
  • the position and attitude transformation of the static platform is carried out, so that the static platform can be kept smooth during the motion process, and the motion precision of the static platform can be guaranteed.
  • the arc motion transformation with the far-centered fixed point as the center and the linear motion transformation pointing to the center of the circle can be performed simultaneously; in the attitude transformation, the rotation transformation of the static coordinate system around the Z axis of the mechanical coordinate system and the static coordinate transformation The rotation transformation of the system around its own Y axis can also be performed at the same time.
  • the goal of position interpolation is to move the static coordinate system from the spatial position before reconstruction to the target position S 1 after reconstruction along a certain track from S 0 shown in Figure 9 .
  • the position interpolation process is divided into two movements.
  • Motion 1 circular motion with the far center point as the center and r0 as the radius
  • motion 2 linear motion along the radius r pointing to the center of the circle, that is, the motion with the radius r0 constantly changing.
  • the motion trajectory formed after the combination of the two is shown in Figure 9 in the motion trajectory of the static coordinate system, which is a curved motion with a radius changing from r0 to r1.
  • variable ⁇ r that symbolizes circular motion changes from 0 to ⁇ with the increment of ⁇
  • variable r that symbolizes linear motion takes ⁇ r as the change amount
  • the vector is the positive direction of the XF axis
  • the result is the positive direction of the ZF axis
  • the positive direction of the YF axis satisfies the right-hand rule.
  • the apocentric point coordinate system O F -X F Y F Z F shown in Figure 3 is established.
  • the i The coordinates of each interpolation point are:
  • location_pre(i) [r ⁇ cos ⁇ r ; r ⁇ sin ⁇ r ; 0; 1]
  • the pose matrix T_machine_dm of the telecentric point coordinate system O F -X F Y F Z F in the machine coordinate system is recorded as:
  • the parameters can be obtained as follows, where fix is the coordinate of the telecentric point before reconstruction in the static coordinate system, which is a known quantity:
  • the goal of attitude interpolation is to transform the attitude before reconstruction of the static coordinate system into the reconstructed target attitude according to certain rules, as shown in Figure 10.
  • the Y0 axis of the static coordinate system needs to be parallel to the XY plane of the global coordinate system at all times, so as shown in Figure 9, the attitude transformation is divided into two parts: 1The static coordinate system rotates around the global Z axis ;2The static coordinate system rotates around its own Y0 axis. Therefore, first of all, two rotation angles should be obtained by reconstructing the posture relationship of the static coordinate system before and after, and then the two angles are linearly interpolated, and finally a series of continuous transformation arrays composed of each interpolation point.
  • R_rotate_posture represents the rotation matrix of the i-th interpolation point in the global coordinate system
  • R_machine_s_begin is the rotation transformation matrix of the static coordinate system before reconstruction, which is composed of 9 elements in rows 1-3 and columns 1-3 of the transformation matrix T_machine_s_begin
  • R_z Indicates the transformation process around the global Z axis
  • R_y represents the transformation process around the static coordinate system Y 0 axis.
  • R R_machine_s_end ⁇
  • R_y -1 R_z ⁇ R_machine_s_begin (31)
  • R_machine_s_end is the rotation matrix of the static coordinate system after reconstruction, which is composed of 9 elements in rows 1-3 and columns 1-3 of the transformation matrix T_machine_s_end;
  • performing position transformation and attitude transformation on the static platform according to the intermediate pose and the current pose of the moving platform includes the following steps:
  • Step S1110 Transform the intermediate pose into the ninth transformation matrix between the static coordinate system and the mechanical coordinate system and the tenth transformation matrix between the static coordinate system and the dynamic coordinate system;
  • the static coordinate system is the coordinate system where the static platform is located, and the dynamic coordinate system system is the coordinate system where the moving platform is located;
  • Step S1120 Transform the attitude parameters of the joints of the passive arm according to the ninth transformation matrix and the tenth transformation matrix, wherein the passive arm is connected to the static platform;
  • Step S1130 Obtain the second stretching amount of the telescopic element according to the ninth transformation matrix and the tenth transformation matrix, and adjust the telescopic element according to the second stretching amount, so that the current pose of the moving platform in the mechanical coordinate system remains unchanged.
  • the position transformation and posture transformation of the static platform according to the intermediate pose and the current pose of the moving platform need to be realized through the transformation of the posture parameters of the joints of the passive arm and the telescopic adjustment of the telescopic elements.
  • the posture parameters of the static platform and the second stretching amount of the telescopic element are used to perform intermediate pose transformation, so that the static platform remains smooth during the motion process and ensures the motion accuracy of the static platform.
  • the driving parameters of the joints of the passive arm and the driving parameters of the telescopic element can be further analyzed to realize the attitude parameters of the joints of the passive arm and the second stretching amount of the stretching element.
  • the analysis of the attitude parameters of the joints of the passive arm and the second expansion and contraction amount of the expansion element may be referred to as pose interpolation kinematics calculation.
  • the parameters obtained by (1) position interpolation and (2) attitude interpolation are converted into a conversion matrix describing the pose of the static coordinate system and a conversion matrix between dynamic and static coordinate systems, and then respectively solved for the series joint arm and On the motion drive of the Stewart platform, the required drive parameters are finally obtained.
  • T_s_m_i T_machine_s_i -1 ⁇ T_machine_m_mid (38)
  • T_machine_m_mid T_machine_s_begin ⁇ T_s_m_input.
  • T_4_9 i represents the conversion matrix between the 4th joint coordinate system and the 9th joint coordinate system of the i-th interpolation point.
  • ⁇ 7i arcsin[T_4_9 i (2,3) ⁇ cos ⁇ 5 -T_4_9 i (1,3) ⁇ sin ⁇ 5 ] (41)
  • ⁇ 5i arccos(T_4_9 i (2,4)/d 6 ) (46)
  • the relative coordinates of the dynamic hinge point in the dynamic coordinate system and the static hinge point in the static coordinate system are calculated.
  • the coordinate value at this time is ( rs , 0).
  • ⁇ si is the rotation angle from the intersection point to the corresponding hinge point with the origin of the static coordinate system as the center.
  • the origin of the static platform coordinate system is defined at the center of the plane where the static hinge point is located, so the ZS axial coordinate of any static hinge point in the static coordinate system is 0, and considering the coordinate rotation and translation transformation, it can be obtained that the static hinge point is on the static platform.
  • the tandem arm and the Stewart platform respectively read the parameters in the array dh_update_po and the array L_mark_po sequentially, so that the static platform can move to a position parallel to the moving platform with a determined trajectory when the pose of the moving platform is stationary relative to the mechanical coordinate system.
  • the "pose transformation phase" is completed.
  • performing rotation transformation on the moving platform according to the twist angle so that the parallel platform returns to zero position includes the following steps:
  • Step S1310 interpolating the twist angle to obtain the intermediate angle of the moving platform during the rotation transformation process
  • Step S1320 Perform rotation transformation on the moving platform according to the intermediate angle and the current angle of the moving platform.
  • performing rotation transformation on the moving platform according to the intermediate angle and the current angle of the moving platform includes the following steps:
  • Step S1410 Obtain the eleventh transformation matrix between the static coordinate system and the dynamic coordinate system according to the intermediate angle, obtain the third stretching amount of the telescopic element according to the eleventh transformation matrix, and adjust the telescopic element according to the third stretching amount, so as to Return the parallel platform to zero position.
  • the torsion angle can also be interpolated to obtain the intermediate angle of the moving platform during the rotation transformation process, and the intermediate angle can be converted to obtain the difference between the static coordinate system and the dynamic coordinate system.
  • the eleventh conversion matrix among them is obtained, and then the third stretching amount of the telescopic element is obtained. After the telescopic element is adjusted, the moving platform returns to the zero position, and the parallel platform returns to the zero position.
  • the third telescopic amount can be converted into a driving parameter of the telescopic element, and the driving adjustment of the telescopic element can be performed.
  • the method for controlling the operation field center of the reconstruction instrument further includes the following steps:
  • Step S1510 After the parallel platform returns to the zero position, according to the conversion relationship between the static coordinate system and the mechanical coordinate system after the reconstruction of the surgical field, the telecentric fixed point is obtained after the reconstruction of the surgical field.
  • the coordinates in the static coordinate system, wherein the coordinates of the telecentric fixed point in the mechanical coordinate system remain unchanged.
  • the coordinates of the telecentric fixed point in the static coordinate system after reconstruction of the surgical field can be reconfirmed, so as to facilitate the confirmation of the surgical position.
  • the pose of the static coordinate system has met the pose requirements after reconstruction, and there is still a rotation of the Z-axis of the orbiting coordinate system between the Stewart platform's dynamic coordinate system and the static coordinate system, so in this stage
  • the tandem arm remains stationary, and the Stewart platform interpolates the rotation angle around its own Z axis described above and solves the corresponding driving parameters of Stewart.
  • the motion of this stage is: the moving platform rotates around its own Z axis, and the total angle is ⁇ z . Then, relative to the zero position state, during the rotation process, the Euler angle description between the dynamic and static coordinate systems should be - ⁇ z .
  • the Euler angle description eula_z between the dynamic and static coordinate systems of the i-th interpolation point should be:
  • the conversion matrix T_s_m_rota_z_i between dynamic and static coordinate systems of the i-th interpolation point is:
  • L_mark_re_update [L_mark_po; L_mark_rota_z] (54)
  • the DH parameters of the joint arm in series during the reconstruction process are the parameters for pose interpolation:
  • the Stewart platform After the Stewart platform completes the "posture transformation stage", it reads the array parameters in the array L_mark_rota_z in sequence, so that the moving coordinate system can rotate ⁇ z around its own Z axis, and Stewart returns to the zero position. At this point, the "moving coordinate system return to zero stage” is completed, that is, the entire surgical field reconstruction process is completed.
  • the coordinates of the telecentric fixed point in the static coordinate system need to be obtained again and input to the system.
  • the pose of the static platform changes, but the coordinates of the telecentric fixed point remain unchanged in the mechanical coordinate system.
  • the telecentric fixed point can be obtained The coordinates in the static coordinate system after reconstructing the surgical field are recorded as fix_new:
  • fix_new T_machine_s_end -1 ⁇ T_machine_s_begin ⁇ fix (56)
  • control steps for reconstructing the center of the surgical field of the instrument are as follows:
  • step 4 At the same time as step 3, that is, during the interpolation process, the interpolation point is obtained according to the pose information of the moving platform in the mechanical coordinate system and the attitude matrix of the interpolated pose of the static platform in the mechanical coordinate system
  • the transformation matrix T_s_m between the dynamic and static coordinate systems is used to calculate the length L_mark_po of the six driving rods of the Stewart platform according to the inverse kinematics of the Stewart platform, so that the pose of the moving platform in the mechanical coordinate system remains static during the movement of the static platform.
  • the Stewart platform cannot be completely returned to zero in the pose transformation stage, and there may be a twist in the Z-axis direction between the dynamic and static platforms. Due to the limitation of the degree of freedom of the passive arm, it cannot By adjusting the attitude of the static platform to make the Stewart platform return to zero, so choose to control the brake platform to rotate around the Z axis to make the Stewart platform return to zero. The rotation of the moving platform will cause the instrument to rotate around the Z axis, but this will not violate the principle of telecentric fixed point, nor will it change the center of the surgical field of the instrument, which can effectively complete the reconstruction of the center of the surgical field.
  • the pose matrix of the dynamic and static coordinate system under the mechanical coordinate system after the pose transformation stage is completed calculate the torsion angle between the dynamic and static, perform multi-point interpolation on the angle, and then calculate each difference point according to the inverse kinematics of the Stewart platform
  • the length L_mark_po of the six driving rods of the Stewart platform makes the moving platform rotate around the Z axis at a corresponding speed until the Stewart platform returns to the zero position.
  • This embodiment also provides a method for controlling a robotic arm, wherein the robotic arm includes a parallel platform, and the parallel platform includes a dynamic platform, a static platform, and a telescopic element connecting the dynamic platform and the static platform, as shown in FIG. 16 , the method includes the following step:
  • Step S1610 Obtain the current pose of the moving platform in the mechanical coordinate system, wherein the mechanical coordinate system is the global coordinate system of the mechanical arm where the moving platform is located;
  • Step S1620 Obtain the target pose of the static platform in the mechanical coordinate system according to the current pose of the moving platform.
  • Step S1630 Perform pose transformation on the static platform according to the current pose of the moving platform and the target pose of the static platform.
  • the target pose of the static platform in the machine coordinate system may be: the center of the target pose of the static platform is on the central axis of the current pose of the moving platform, and the static platform in the target pose is parallel to the moving platform.
  • the robotic arm also includes a passive arm connected to the static platform.
  • the pose transformation of the static platform includes the following steps:
  • Step S1710 Determine the driving parameters for controlling the passive arm according to the current pose of the moving platform and the target pose of the static platform;
  • Step S1720 Based on the driving parameters, by controlling the passive arm, perform pose transformation on the static platform.
  • the passive arm includes multiple joints in series, as shown in Figure 18, according to the current pose of the moving platform and the target pose of the static platform, the step of performing pose transformation on the static platform includes: Step S1810: According to the dynamic platform The current pose of the static platform and the target pose of the static platform, the pose transformation of the static platform is carried out through the passive arm, wherein, when the attitude parameters of the joints of the passive arm are transformed, the attitude parameters of some joints in the joints in series unchanged, change the attitude parameters of another part of joints in multiple series.
  • the passive arm includes a first joint, a second joint, and a third joint connected in series, wherein the attitude parameters of the first joint and the third joint are constant, and the third joint is connected to the static platform.
  • performing pose transformation on the static platform further includes: during the pose transformation process of the static platform, the current pose of the moving platform in the mechanical coordinate system remains unchanged.
  • performing pose transformation on the static platform further includes: obtaining the first telescopic amount of the telescopic element according to the current pose of the moving platform and the target pose of the static platform, and when performing pose transformation on the static platform, according to the first telescopic Adjust the telescopic elements to keep the current pose of the moving platform in the mechanical coordinate system unchanged.
  • the pose transformation of the static platform includes the following steps: obtaining the original pose of the static platform, interpolating the original pose and the target pose of the static platform, and obtaining the intermediate pose of the static platform during the pose transformation process ; and perform position transformation and attitude transformation on the static platform according to the intermediate pose and the current pose of the moving platform.
  • the position transformation includes circular motion transformation with the far-centered fixed point as the center and the linear motion transformation pointing to the center of the circle.
  • the attitude transformation includes the rotation transformation of the static coordinate system around the Z axis of the machine coordinate system and the static coordinate system around its own Y axis. rotation transformation.
  • position transformations and pose transformations can be performed simultaneously.
  • control method of the mechanical arm further includes the following steps: according to the current pose of the moving platform and the target pose of the static platform, obtaining the twist angle between the moving platform and the static platform on the central axis, and adjusting the moving platform according to the twist angle
  • the platform undergoes a rotational transformation to return the parallel platform to zero position.
  • the current pose of the moving platform is: the pose of the moving platform in the mechanical coordinate system after reconstructing the center of the instrument’s operating field;
  • the target pose of the static platform is: after rebuilding the center of the operating field of the instrument, the static platform is The pose in the coordinate system.
  • This embodiment also provides a control system for reconstructing the center of the surgical field of the instrument, which is used to implement the above-mentioned embodiments and preferred implementation modes, and those that have already been described will not be repeated.
  • the term "module” and the like may be a combination of software and/or hardware that realizes a predetermined function.
  • the systems described in the following embodiments are preferably implemented in software, implementations in hardware, or a combination of software and hardware are also possible and contemplated.
  • Fig. 19 is a structural block diagram of the control system of the surgical field center of the reconstruction instrument according to an embodiment of the present disclosure. As shown in Fig. 19, the system includes:
  • the first acquisition module 210 is configured to acquire the current pose of the moving platform in the mechanical coordinate system after the center of the surgical field of the instrument is reconstructed, wherein the mechanical coordinate system is the global coordinate system of the mechanical arm where the moving platform is located;
  • the second acquisition module 220 is used to acquire the target pose of the static platform in the mechanical coordinate system after reconstructing the surgical field center of the instrument according to the current pose of the moving platform, wherein the moving platform and the static platform are connected by telescopic elements in the mechanical arm
  • the center of the target pose of the static platform is on the central axis of the current pose of the moving platform, and the static platform in the target pose is parallel to the moving platform;
  • the first conversion module 230 is used to perform pose transformation on the static platform according to the current pose of the moving platform and the target pose of the static platform;
  • the second transformation module 240 is used to obtain the torsion angle between the moving platform and the static platform on the central axis according to the current pose of the moving platform and the target pose of the static platform, and perform rotation transformation on the moving platform according to the twisting angle, so that The parallel platform returns to zero position.
  • the first transformation module 230 is also used to obtain the first transformation between the static coordinate system and the dynamic coordinate system when the reconstructed and returned to zero state is obtained according to the current pose of the moving platform and the target pose of the static platform. Transformation matrix; the static coordinate system is the coordinate system where the static platform is located, and the dynamic coordinate system is the coordinate system where the moving platform is located; obtain the second transformation matrix between the static coordinate system and the dynamic coordinate system when the reconstruction has not returned to the zero state; And the third transformation matrix between the mechanical coordinate system and the static coordinate system before reconstruction; obtain the fourth transformation matrix between the reconstructed mechanical coordinate system and the static coordinate system according to the first transformation matrix, the second transformation matrix and the third transformation matrix; according to The fourth transformation matrix obtains the fifth transformation matrix between the joint coordinate systems of the passive arm in the mechanical arm, and transforms the attitude parameters of the joints of the passive arm according to the fifth transformation matrix, wherein the passive arm is connected to the static platform.
  • the passive arm includes a first joint, a second joint and a third joint connected in series, wherein the attitude parameters of the first joint and the third joint are unchanged, and the third joint is connected to the static platform;
  • the first transformation module 230 is also used to obtain the sixth transformation matrix from the mechanical coordinate system to the coordinate system of the first joint, and the seventh transformation matrix between the coordinate system of the second joint and the coordinate system of the third joint; according to the fifth transformation
  • the matrix, the sixth transformation matrix and the seventh transformation matrix acquire an eighth transformation matrix between the first joint and the second joint; transform the attitude parameters of the second joint according to the eighth transformation matrix.
  • the first transformation module 230 is also used to obtain the first stretching amount of the telescopic element according to the current pose of the moving platform and the target pose of the static platform.
  • the first A stretching amount is used to adjust the stretching element so that the current pose of the moving platform in the mechanical coordinate system remains unchanged.
  • the first transformation module 230 is also used to obtain the original pose of the static platform before reconstruction, interpolate the original pose and the target pose of the static platform, and obtain the pose transformation process of the static platform
  • the position transformation and attitude transformation of the static platform are performed, wherein the position transformation includes the circular motion transformation with the far-centered fixed point as the center of the circle and the linear motion pointing to the center of the circle Transformation
  • posture transformation includes the rotation transformation of the static coordinate system around the Z axis of the machine coordinate system and the rotation transformation of the static coordinate system around its own Y axis.
  • the first transformation module 230 is also used to transform the intermediate pose into the ninth transformation matrix between the static coordinate system and the mechanical coordinate system and the tenth transformation matrix between the static coordinate system and the dynamic coordinate system;
  • the coordinate system is the coordinate system where the static platform is located
  • the dynamic coordinate system is the coordinate system where the dynamic platform is located;
  • the attitude parameters of the joints of the passive arm are transformed according to the ninth transformation matrix and the tenth transformation matrix, wherein the passive arm is connected to the static platform ;
  • the ninth transformation matrix and the tenth transformation matrix the second stretching amount of the telescopic element is obtained, and the telescopic element is adjusted according to the second stretching amount, so that the current pose of the moving platform in the mechanical coordinate system remains unchanged.
  • the second transformation module 240 is also used to interpolate the twist angle to obtain the intermediate angle of the moving platform during the rotation transformation process; obtain the tenth angle between the static coordinate system and the dynamic coordinate system according to the intermediate angle.
  • a transformation matrix, according to the eleventh transformation matrix, the third stretching amount of the telescopic element is obtained, and the telescopic element is adjusted according to the third stretching amount, so that the parallel platform returns to zero.
  • An embodiment of the present disclosure also provides a control system for a robotic arm, wherein the robotic arm includes a parallel platform, and the parallel platform includes a dynamic platform, a static platform, and a telescopic element connecting the dynamic platform and the static platform, as shown in FIG. 20 , the system Can include:
  • the first obtaining module 410 is used to obtain the current pose of the moving platform in the mechanical coordinate system, wherein the mechanical coordinate system is the global coordinate system of the mechanical arm where the moving platform is located;
  • the second acquiring module 420 is used to acquire the target pose of the static platform in the mechanical coordinate system according to the current pose of the moving platform;
  • the first transformation module 430 is configured to perform pose transformation on the static platform according to the current pose of the moving platform and the target pose of the static platform.
  • the mechanical arm further includes a passive arm, and the passive arm is connected to the static platform, wherein the first transformation module 430 is also used for: according to the current pose of the moving platform and the target pose of the static platform , determining a driving parameter for controlling the passive arm; and performing pose transformation on the static platform by controlling the passive arm based on the driving parameter.
  • the mechanical arm also includes a passive arm, the passive arm is connected to the static platform, the passive arm includes a plurality of joints connected in series, and when the attitude parameters of the joints of the driven arm are transformed, the joints in the plurality of series connected The attitude parameters of some joints remain unchanged, and the attitude parameters of another part of the multiple joints in series are changed.
  • the passive arm includes a first joint, a second joint and a third joint connected in series, wherein the attitude parameters of the first joint and the third joint are unchanged, and the third joint is connected to the static platform.
  • the current pose of the moving platform in the mechanical coordinate system remains unchanged during the pose transformation process of the static platform.
  • the first transformation module 430 is also used to: obtain the first stretching amount of the telescopic element according to the current pose of the moving platform and the target pose of the static platform, and when performing pose transformation on the static platform, according to The first telescopic amount adjusts the telescopic element so that the current pose of the moving platform in the mechanical coordinate system remains unchanged.
  • the first transformation module 430 is also used to: obtain the original pose of the static platform, perform interpolation on the original pose and the target pose of the static platform, and obtain the intermediate position of the static platform during the pose transformation process. Pose; and perform position transformation and attitude transformation on the static platform according to the intermediate pose and the current pose of the moving platform.
  • the position transformation includes a circular motion transformation with the far-centered fixed point as the center and a linear motion transformation pointing to the center of the circle
  • the attitude transformation includes the rotation transformation of the static coordinate system around the Z-axis of the mechanical coordinate system and the static coordinate system A rotation transform around its own Y axis.
  • the position transformation and pose transformation are performed simultaneously.
  • the target pose of the static platform is as follows: the center of the target pose of the static platform is on the central axis of the current pose of the moving platform, and the static platform in the target pose is parallel to the moving platform.
  • control system further includes: a second transformation module 440, configured to obtain the twist on the central axis between the moving platform and the static platform according to the current pose of the moving platform and the target pose of the static platform Angle, according to the twist angle, the moving platform is rotated and transformed to make the parallel platform return to zero.
  • a second transformation module 440 configured to obtain the twist on the central axis between the moving platform and the static platform according to the current pose of the moving platform and the target pose of the static platform Angle, according to the twist angle, the moving platform is rotated and transformed to make the parallel platform return to zero.
  • an embodiment of the present disclosure also provides a control device 300 for reconstructing the surgical field center of an instrument, including a robotic arm 310 and a control device 320 ;
  • the control device 320 is used to perform pose transformation on the static platform on the robotic arm 310 according to the control method for reconstructing the surgical field center of the instrument in the first aspect above, and perform rotation transformation on the moving platform on the robotic arm 310, or according to the above-mentioned second aspect
  • the control method of the mechanical arm performs pose transformation on the static platform on the mechanical arm 310 .
  • the above-mentioned control device 300 of the surgical field center of the reconstruction instrument performs pose transformation on the static platform on the mechanical arm 310 through the control device 320, and performs rotation transformation on the dynamic platform on the mechanical arm 310, realizing the return of the parallel platform to zero position and solving the problem of In the related art, it is difficult for the parallel robot to realize the attitude return to zero, which improves the control accuracy of the center of the surgical field of the reconstructed instrument and ensures the motion performance of the parallel platform.
  • the embodiment of the present disclosure also provides a mechanical arm assembly 500, the mechanical arm assembly includes: a mechanical arm 510; and a control device 520, the control device is used to: Perform pose transformation on the static platform on the manipulator, perform rotation transformation on the moving platform on the manipulator, or perform pose transformation on the static platform on the manipulator according to the above-mentioned control method of the manipulator.
  • an embodiment of the present disclosure also provides a surgical robot.
  • the surgical robot includes: a base 610 ; and a control device at the center of the surgical field of the above-mentioned reconstruction instrument or the above-mentioned robotic arm assembly.
  • An embodiment of the present disclosure also provides a readable storage medium, on which an executable program is stored.
  • the executable program is executed by a processor, the control method for the operation field center of the reconstructed instrument or the control method for the robotic arm are realized. .
  • the above-mentioned readable storage medium can realize the zero position return of the parallel platform by running the executable program on the processor, which solves the problem that the attitude of the parallel robot is difficult to return to zero in the related technology, and improves the control of the center of the surgical field of the reconstruction instrument Accuracy ensures the kinematic performance of the parallel platform.
  • Embodiments of the present disclosure may also provide a computer program product, where the computer program product includes computer instructions.
  • the computer instructions are executed by a processor, the above-mentioned method for controlling the surgical field center of the reconstruction instrument or the above-mentioned control method for the robotic arm are realized. .
  • the program can be stored in a readable storage medium.
  • the storage medium includes: ROM/RAM, magnetic disk, optical disk, etc.

Abstract

一种重建器械术野中心的控制方法、系统和可读存储介质，获取重建器械术野中心后，动平台在机械坐标系下的当前位姿（S110），根据动平台的当前位姿获取重建器械术野中心后，静平台在机械坐标系下的目标位姿（S120），根据动平台的当前位姿和静平台的目标位姿对静平台进行位姿变换（S130）；根据动平台的当前位姿和静平台的目标位姿获取动平台和静平台之间在中心轴线上的扭转角度，根据扭转角度对动平台进行旋转变换，以使并联平台回归零位（S140）。

Description

重建器械术野中心的控制方法、系统和存储介质
相关申请的交叉引用
本公开要求于2021年06月11日提交的申请号为202110682456.0的中国专利申请的优先权，其全部内容通过引用并入本文。
技术领域
本公开涉及机器人技术领域，特别是涉及重建器械术野中心的控制方法、系统、设备和存储介质。
背景技术
机器人在进行应用操作前需要进行摆位，使器械末端到达相应的操作空间，对并联机器人来说，摆位首先依赖于串联机械臂的运动，使器械末端到达操作空间，再通过操控并联平台使器械末端到达操作空间的中间位置，实现精确的摆位。
在将器械转移到另一较远的位置时，若直接通过主动臂将器械移至该目标点，此时并联平台可能已经处于工作空间的边缘位置，因此需要在器械位置不变的情况下，将并联平台调整至零位状态。
并联机器人的主动臂运动范围小，在完成摆位后，并联平台通常不处于零点位置，这可能导致并联平台在运动空间中无法覆盖应用的操作空间，从而使并联平台卡在极限位置无法完成操作。由于被动臂的运动自由度有限，仅通过被动臂的运动无法完全实现姿态回零。
发明内容
本公开实施例提供了一种重建器械术野中心的控制方法、系统、设备和可读存储介质。
第一方面，本公开实施例提供了一种重建器械术野中心的控制方法，包括：
获取重建器械术野中心后，动平台在机械坐标系下的当前位姿，其中，机械坐标系是动平台所在机械臂的全局坐标系；
根据动平台的当前位姿获取重建器械术野中心后，静平台在机械坐标系下的目标位姿，其中，动平台和静平台是机械臂中通过伸缩元件连接的并联平台，静平台的目标位姿的中心在动平台的当前位姿的中心轴线上，且处于目标位姿的静平台与动平台平行；
根据动平台的当前位姿和静平台的目标位姿对静平台进行位姿变换；
根据动平台的当前位姿和静平台的目标位姿获取动平台和静平台之间在中心轴线上的扭转角度，根据扭转角度对动平台进行旋转变换，以使并联平台回归零位。
第二方面，本公开实施例提供了一种机械臂的控制方法，其中，所述机械臂包括并联平台，所述并联平台包括动平台、静平台和连接所述动平台和所述静平台的伸缩元件，所述方法包括以下步骤：获取所述动平台在机械坐标系下的当前位姿，其中，所述机械坐标系是所述动平台所在所述机械臂的全局坐标系；根据所述动平台的当前位姿，获取所述静平台在所述机械坐标系下的目标位姿，以及根据所述动平台的当前位姿和所述静平台的目标位姿，对所述静平台进行位姿变换。
第三方面，本公开实施例提供了一种机械臂的控制方法，其中，所述方法包括以下步骤：获取动平台在机械坐标系下的当前位姿，其中，所述机械坐标系是所述动平台所在机械臂的全局坐标系；根据所述动平台的当前位姿获取静平台在所述机械坐标系下的目标位姿，其中，所述动平台和所述静平台是所述机械臂中通过伸缩元件连接的并联平台；根据所述动平台的当前位姿和所述静平台的目标位姿对所述静平台进行位姿变换。
第四方面，本公开实施例提供了一种重建器械术野中心的控制系统，包括：
第一获取模块，用于获取重建器械术野中心后，动平台在机械坐标系下的当前位姿，其中，机械坐标系是动平台所在机械臂的全局坐标系；
第二获取模块，用于根据动平台的当前位姿获取重建器械术野中心后，静平台在机械坐 标系下的目标位姿，其中，动平台和静平台是机械臂中通过伸缩元件连接的并联平台，静平台的目标位姿的中心在动平台的当前位姿的中心轴线上，且处于目标位姿的静平台与动平台平行；
第一变换模块，用于根据动平台的当前位姿和静平台的目标位姿对静平台进行位姿变换；
第二变换模块，用于根据动平台的当前位姿和静平台的目标位姿获取动平台和静平台之间在中心轴线上的扭转角度，根据扭转角度对动平台进行旋转变换，以使并联平台回归零位。
第五方面，本公开实施例提供了一种机械臂的控制系统，其中，所述机械臂包括并联平台，所述并联平台包括动平台、静平台和连接所述动平台和所述静平台的伸缩元件，所述系统包括：第一获取模块，用于获取所述动平台在机械坐标系下的当前位姿，其中，所述机械坐标系是所述动平台所在所述机械臂的全局坐标系；第二获取模块，用于根据所述动平台的当前位姿，获取所述静平台在所述机械坐标系下的目标位姿；以及第一变换模块，用于根据所述动平台的当前位姿和所述静平台的目标位姿，对所述静平台进行位姿变换。
第六方面，本公开实施例提供了一种机械臂组件，所述机械臂组件包括：机械臂；以及控制装置，所述控制装置用于：根据上述第一方面的重建器械术野中心的控制方法对所述机械臂上的静平台进行位姿变换，对所述机械臂上的动平台进行旋转变换，或者根据上述第二方面的机械臂的控制方法对所述机械臂上的静平台进行位姿变换。
第七方面，本公开实施例提供了一种手术机器人，所述手术机器人包括：底座；以及如上述第六方面的机械臂组件。
第八方面，本公开实施例提供了一种可读存储介质，其上存储有可执行程序，该可执行程序被处理器执行时实现如上述第一方面的重建器械术野中心的控制方法或者如上述第二方面的机械臂的控制方法。
第九方面，本公开实施例提供了一种计算机程序产品，其包括计算机指令，其中，所述计算机指令被处理器执行时实现如上述第一方面的重建器械术野中心的控制方法或者如上述第二方面的机械臂的控制方法。
相比于相关技术，本公开实施例提供的重建器械术野中心的控制方法、系统、设备和可读存储介质，获取重建器械术野中心后，动平台在机械坐标系下的当前位姿，根据动平台的当前位姿获取重建器械术野中心后，静平台在机械坐标系下的目标位姿，根据动平台的当前位姿和静平台的目标位姿对静平台进行位姿变换；根据动平台的当前位姿和静平台的目标位姿获取动平台和静平台之间在中心轴线上的扭转角度，根据扭转角度对动平台进行旋转变换，以使并联平台回归零位，解决了相关技术中存在的并联机器人难以实现姿态回零的问题，提升了重建器械术野中心的控制准确度，保证并联平台的运动性能。
本公开的一个或多个实施例的细节在以下附图和描述中提出，以使本公开的其他特征、目的和优点更加简明易懂。
附图说明
此处所说明的附图用来提供对本公开的进一步理解，构成本公开的一部分，本公开的示意性实施例及其说明用于解释本公开，并不构成对本公开的不当限定。在附图中：
图1是根据本公开实施例的重建器械术野中心的控制方法的流程示意图；
图2是根据本公开实施例的重建器械术野中心的控制方法的应用示意图；
图3是根据本公开实施例的机械臂的关节示意图；
图4是根据本公开实施例的对静平台进行位姿变换的步骤的第一示例的流程示意图；
图5是根据本公开实施例的对被动臂的关节的姿态参数进行变换的步骤的流程示意图；
图6是根据本公开实施例的对第二关节的驱动参数进行变换的步骤的流程示意图；
图7是根据本公开实施例的对静平台进行位姿变换的步骤的第二示例的流程示意图；
图8是根据本公开实施例的对静平台进行位姿变换的步骤的第三示例的流程示意图；
图9是根据本公开实施例的并联平台中静平台的位姿变换示意图；
图10是根据本公开实施例的并联平台中静平台的姿态变换原理图；
图11是根据本公开实施例的对静平台进行位置变换和姿态变换的步骤的流程示意图；
图12是根据本公开实施例的并联平台的系统默认初始位置示意图；
图13是根据本公开实施例的对动平台进行旋转变换的步骤的第一示例的流程示意图；
图14是根据本公开实施例的对动平台进行旋转变换的步骤的第二示例的流程示意图；
图15是根据本公开实施例的获取远心不动点在重建术野后静坐标系下的坐标的步骤的流程示意图；
图16是根据本公开实施例的机械臂的控制方法的流程示意图；
图17是根据本公开实施例的机械臂的控制方法中对静平台进行位姿变换的步骤的流程示意图；
图18是根据本公开实施例的机械臂的控制方法中对被动臂的关节的姿态参数进行变换的步骤的流程示意图；
图19是根据本公开实施例的重建器械术野中心的控制系统的结构框图；
图20是根据本公开实施例的机械臂的控制系统的结构框图；
图21是根据本公开实施例的重建器械术野中心的控制设备的结构框图；
图22是根据本公开实施例的机械臂组件的结构框图；
图23是根据本公开实施例的手术机器人的结构框图。
具体实施方式
为了使本公开的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本公开进行描述和说明。应当理解，此处所描述的具体实施例仅仅用以解释本公开，并不用于限定本公开。基于本公开提供的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本公开保护的范围。
在本公开中提及“实施例”意味着，结合实施例描述的特定特征、结构或特性可以包含在本公开的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例，也不是与其它实施例互斥的独立的或备选的实施例。本领域普通技术人员显式地和隐式地理解的是，本公开所描述的实施例在不冲突的情况下，可以与其它实施例相结合。
除非另作定义，本公开所涉及的技术术语或者科学术语应当为本公开所属技术领域内具有一般技能的人士所理解的通常意义。本公开所涉及的“一”、“一个”、“一种”、“该”等类似词语并不表示数量限制，可表示单数或复数。本公开所涉及的术语“包括”、“包含”、“具有”以及它们任何变形，意图在于覆盖不排他的包含；例如包含了一系列步骤或模块(单元)的过程、方法、系统、产品或设备没有限定于已列出的步骤或单元，而是可以还包括没有列出的步骤或单元，或可以还包括对于这些过程、方法、产品或设备固有的其它步骤或单元。本公开所涉及的“连接”、“相连”、“耦接”等类似的词语并非限定于物理的或者机械的连接，而是可以包括电气的连接，不管是直接的还是间接的。本公开所涉及的“多个”是指两个或两个以上。“和/或”描述关联对象的关联关系，表示可以存在三种关系，例如，“A和/或B”可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。字符“/”一般表示前后关联对象是一种“或”的关系。本公开所涉及的术语“第一”、“第二”、“第三”等仅仅是区别类似的对象，不代表针对对象的特定排序。
本实施例提供了一种重建器械术野中心的控制方法。图1是根据本公开实施例的重建器械术野中心的控制方法的流程示意图，如图1所示，该流程包括如下步骤：
步骤S110：获取重建器械术野中心后，动平台在机械坐标系下的当前位姿，其中，机械坐标系是动平台所在机械臂的全局坐标系；
在本步骤中，器械术野中心是指器械在手术视野中心的位置，重建器械术野中心是指并联机器人的机械臂末端的术野中心非目标区域，需要在不改变并联机器人的远心不动点的情况下重新确定，使术野中心在目标区域；在重建器械术野中心的过程中，一般是通过并联机器人的并联平台上的动平台的移动来实现术野中心的转移，但此时并联平台上的动平台和静平台已不处于零点位置；机械臂自身所在的全局坐标系是适用于机械臂各个部件的机械坐标 系，动平台和静平台的位姿映射到机械坐标系上，有利于两者之间的位姿转换计算。
具体的，机械坐标系可以是图3中的整个机械臂所在的全局坐标系，该全局坐标系的原点可为机械臂的底座中心。
步骤S120：根据动平台的当前位姿获取重建器械术野中心后，静平台在机械坐标系下的目标位姿，其中，动平台和静平台是机械臂中通过伸缩元件连接的并联平台，静平台的目标位姿的中心在动平台的当前位姿的中心轴线上，且处于目标位姿的静平台与动平台平行；
在本步骤中，并联平台是机械臂的重要组成部分，包括动平台、伸缩元件和静平台，由于静平台和动平台在零位时的相对位姿是确定的，静平台的目标位姿的中心在动平台的当前位姿的中心轴线上，且处于目标位姿的静平台与动平台平行，利用重建器械术野中心后动平台的当前位姿可确定静平台在机械坐标系下的目标位姿。
步骤S130：根据动平台的当前位姿和静平台的目标位姿对静平台进行位姿变换；
在本步骤中，利用动平台的当前位姿和静平台的目标位姿作为变换的目标，对静平台进行位姿变换，使静平台运动至目标位姿，在这一过程中，动平台在机械坐标系下的当前位姿保持不变。
步骤S140：根据动平台的当前位姿和静平台的目标位姿获取动平台和静平台之间在中心轴线上的扭转角度，根据扭转角度对动平台进行旋转变换，以使并联平台回归零位；
在本步骤中，在静平台进行位姿变换后，并联平台还无法完全回到零位，动平台和静平台之间还可能存在一个动平台中心轴方向上的扭转，由于机械臂上的被动臂的自由度限制，已无法通过调整静平台的姿态来使并联平台回到零位，故而选择控制动平台绕中心轴线转动来使并联平台回到零位，动平台的转动会导致器械绕中心轴线转动，但这并不会违背远心不动点原则，也不会改变器械的术野中心，可以有效地完成术野中心重建。
在本实施例中，通过上述步骤，获取重建器械术野中心后，动平台在机械坐标系下的当前位姿，根据动平台的当前位姿获取重建器械术野中心后，静平台在机械坐标系下的目标位姿，根据动平台的当前位姿和静平台的目标位姿对静平台进行位姿变换；根据动平台的当前位姿和静平台的目标位姿获取动平台和静平台之间在中心轴线上的扭转角度，根据扭转角度对动平台进行旋转变换，以使并联平台回归零位，解决了相关技术中存在的并联机器人难以实现姿态回零的问题，提升了重建器械术野中心的控制准确度，保证并联平台的运动性能。
需要说明的是，本公开的重建器械术野中心的控制方法可应用于多种应用场景。如图2所示，在前置摆位操作中，前置助手通过移动被动臂各关节来确立远心不动点的同时，建立器械术野中心，使器械末端点的初始位置位于术野的中心。由于该过程器械并未被插入待检物内，而是由前置助手目测大致确定，因此可能与理想的器械术野中心存在较大偏差。当器械被插入待检物内，发现存在较大偏差时，可通过本公开的方案，在不改变远心不动点的条件下重建器械术野中心，以消除偏差，使器械获得理想的活动空间。
又如，受伸缩元件(如动静铰链)摆动角限制，并联平台工作空间相对较小，而某些场景可能要求更大的活动空间。当并联平台当前工作空间无法满足需求，或当前工作空间内已完成阶段性操作时，可通过本公开的方案重建器械术野中心，在不改变远心不动点的条件下移动并联平台工作空间，间接获得更大的活动空间。
在其中一些实施例中，如图4所示，根据动平台的当前位姿和静平台的目标位姿对静平台进行位姿变换包括以下步骤：
步骤S410：根据动平台的当前位姿和静平台的目标位姿获取经重建并回归零位状态时，静坐标系到动坐标系间的第一转换矩阵；静坐标系是静平台所在的坐标系，动坐标系是动平台所在的坐标系；
步骤S420：获取经重建但未回归零位状态时，静坐标系到动坐标系间的第二转换矩阵；以及重建前机械坐标系到静坐标系间的第三转换矩阵；
步骤S430：根据第一转换矩阵、第二转换矩阵和第三转换矩阵获取重建后机械坐标系到静坐标系间的第四转换矩阵；
步骤S440：根据第四转换矩阵获取机械臂中被动臂的关节坐标系间的第五转换矩阵，根据第五转换矩阵对被动臂的关节的姿态参数进行变换，其中，被动臂与静平台连接。
在本实施例中，在重建器械术野中心前后，动坐标系、静坐标系和机械坐标系之间存在转换关系，可通过转换矩阵进行描述。此处采用四种转换矩阵：经重建并回归零位状态时，静坐标系到动坐标系间的第一转换矩阵；经重建但未回归零位状态时，静坐标系到动坐标系间的第二转换矩阵；重建前机械坐标系到静坐标系间的第三转换矩阵；重建后机械坐标系到静坐标系间的第四转换矩阵。第一转换矩阵可根据动平台的当前位姿和静平台的目标位姿得到，第二转换矩阵可根据动平台和静平台之间的转换关系得到，第三转换矩阵可根据机械臂的被动臂的关节信息转换得到，由于动平台的位姿在变换过程中保持不变，第四转换矩阵和第一转换矩阵的乘积与第三转换矩阵和第二转换矩阵的乘积相同，利用这一等价关系，可得到第四转换矩阵，其反映的是机械臂中被动臂至并联平台的转换关系，由此可获得机械臂中被动臂的关节坐标系间的第五转换矩阵，并以第五转换矩阵对被动臂的关节的姿态参数进行变换，通过被动臂的姿态变换实现并联平台中静平台的位姿变换。
需要说明的是，如图2所示，静坐标系是静平台所在的坐标系，重建术野前，以静平台的中心S为原点，Xs轴和Ys轴平行于静平台平面，Zs轴垂直于静平台平面；重建术野后，以静平台的中心S’为原点，X’s轴和Y’s轴平行于静平台平面，Z’s轴垂直于静平台平面；动坐标系是动平台所在的坐标系，重建术野前，以动平台的中心M为原点，X M轴和Y M轴平行于动平台平面，Z M轴垂直于动平台平面；重建术野后，以动平台的中心M’为原点，X’ M轴和Y’ M轴平行于动平台平面，Z’ M轴垂直于动平台平面。
在其中一些实施例中，如图3所示，被动臂10包括串联的第一关节11、第二关节12和第三关节13，其中，第一关节和第三关节的姿态参数不变，第三关节与并联平台20的静平台21连接，动平台22和静平台21通过伸缩元件23连接；
如图5所示，根据第五转换矩阵对被动臂的关节的姿态参数进行变换包括以下步骤：
步骤S510：获取机械坐标系到第一关节的坐标系的第六转换矩阵，以及第二关节的坐标系到第三关节的坐标系间的第七转换矩阵；
步骤S520：根据第五转换矩阵、第六转换矩阵和第七转换矩阵获取第一关节到第二关节间的第八转换矩阵；
步骤S530：根据第八转换矩阵对第二关节的姿态参数进行变换。
在本实施例中，被动臂包括多个串联的关节，在对被动臂的关节的姿态参数进行变换时，其中部分关节的姿态参数不变，只改变其中另一部分关节的姿态参数，由于第一关节和第三关节的姿态参数不变，机械坐标系到第一关节的坐标系的第六转换矩阵，以及第二关节的坐标系到第三关节的坐标系间的第七转换矩阵都是确定的，结合变换目标的第五转换矩阵可得到针对第二关节的姿态参数的第八转换矩阵，基于第八转换矩阵对第二关节的姿态参数进行变换，如此可采用较少的运动关节数量，减少关节的姿态参数的计算量，并尽快实现对被动臂进行调节。
进一步的，第一关节、第二关节和第三关节均可包括一个或多个关节，而且由于第三关节与静平台连接，在完成操作退出并联平台时，保证第三关节充分的运动范围。
在其中一些实施例中，第二关节包括依次连接的第一转动副、第一移动副、第二转动副、第二移动副和第三转动副，所述第一转动副与所述第一关节转动连接，所述第三转动副与所述第三关节转动连接；
如图6所示，所述根据第八转换矩阵对所述第二关节的驱动参数进行变换包括以下步骤：
步骤S610：获取所述第一关节到所述第三转动副的驱动参数变换关系，根据所述第八转换矩阵和所述驱动参数变换关系，求解得到所述第一转动副、所述第一移动副、所述第二转动副、所述第二移动副和所述第三转动副的驱动参数。
在本实施例中，第二关节可包括多个关节，如依次连接的第一转动副、第一移动副、第二转动副、第二移动副和第三转动副，第一转动副与所述第一关节转动连接，所述第三转动 副与所述第三关节转动连接，第二关节包括的多个关节可使被动臂有较大的运动空间，由于第二关节中的各个关节的连接关系，各个关节的驱动参数之间存在一定的变换关系，结合第八转换矩阵，可具体求解出各个关节的具体驱动参数，以此实现被动臂第二关节的调节。
进一步的，在第二关节中，第一转动副的驱动参数可为转动角θ5，第一移动副的驱动参数可为移动距离d6，第二转动副的驱动参数可为转动角θ7，第二移动副的驱动参数可为移动距离d8，第三转动副的驱动参数可为转动角θ9，具体如图3所示。
在其中一些实施例中，如图7所示，根据动平台的当前位姿和静平台的目标位姿对静平台进行位姿变换包括以下步骤：
步骤S710：根据动平台的当前位姿和静平台的目标位姿获取伸缩元件的第一伸缩量，在对静平台进行位姿变换时，根据第一伸缩量对伸缩元件进行调节，以使动平台在机械坐标系下的当前位姿保持不变。
在本实施例中，除了通过被动臂来调整静平台的位姿以外，还需要对动平台和静平台之间的伸缩元件进行调节，以保证在对静平台进行位姿变换时，动平台在机械坐标系下的当前位姿保持不变，只让静平台的位姿进行变换，以实现并联平台的回归零位。
具体的，本公开的方案可应用于Stewart平台，其可关联10个串联的关节。
在具体工作过程中，可划分为“位姿变换阶段”和“动坐标系回归零位阶段”。
串联机械臂的控制通常基于DH参数或改进的DH参数进行坐标系的转换。相连接的两个关节为相邻关节。相邻两个关节的关节坐标系的变换通常采用DH参数或改进的DH参数来表示。以DH参数为例，相邻两个关节坐标系通过绕Z轴旋转θ、平移d后，再绕X轴旋转α、平移a，就能够相互重合，上述的θ、d、α和a即为DH参数。由此可知，如果DH参数越简洁，则相邻两个关节坐标系的转换越简单。
已知各关节的DH参数，根据机器人正运动学可知两相邻关节之间的转换矩阵为
Figure PCTCN2022098189-appb-000001
从第m关节(包含m关节的自由度)到第n关节的转换矩阵可表示为：
Figure PCTCN2022098189-appb-000002
在器械术野中心重建后，根据Stewart平台运动前后动平台在机械坐标系下的位姿不变，可建立如下等式关系：
T_0_4×T_4_9 1×T_9_10×T_s_end_m_end
＝T_machine_s_begin×T_s_begin_m end    (3)
其中，T_0_4表示机械坐标系到第4关节坐标系间转换矩阵；T_4_9 1表示重建并回归零位状态后的第4关节坐标系到第9关节坐标系间转换矩阵；T_9_10表示第9关节坐标系到第10关节坐标系间转换矩阵；T_s_end_m_end表示经重建并回归零位状态时Stewart平台静坐标系到动坐标系间转换矩阵；T_machine_s_begin表示重建前，机械坐标系到静坐标系间转换矩阵；T_s_begin_m_end表示重建后Stewart平台未回归零位状态时，静坐标系到动坐标系间转换矩阵。
由式(3)可得
Figure PCTCN2022098189-appb-000003
其中，等式左侧为包含待求参数的转换矩阵，等式右侧均为已知量，T_4_9 0表示第5-9关节DH参数还未更新时的转换矩阵，T_4_9 1表示重建并回归零位状态后的第4关节坐标系到第9关节坐标系间转换矩阵。由于第1-4、10被动关节臂DH参数整个过程无变动，故据此可求得各矩阵表达式为：
Figure PCTCN2022098189-appb-000004
T_machine_s_begin＝T_0_4×T_4_9 0×T_9_10   (6)
Figure PCTCN2022098189-appb-000005
Figure PCTCN2022098189-appb-000006
式(8)中m oz表示零位状态时，动静坐标系在静坐标系Z轴方向上间距。
令φ x、φ y、φ z分别为绕x、y、z轴的欧拉角，m x、m y、m z分别为任意时刻动坐标系在静坐标系下的坐标位置，则任意时刻静坐标系到动坐标系的转换矩阵为：
Figure PCTCN2022098189-appb-000007
如图3所示，第4关节坐标系到第9关节坐标系的转换矩阵可求得表达式为：
Figure PCTCN2022098189-appb-000008
由等式(4)两侧矩阵对应元素相等，即可求得待更新的第5-9关节DH参数。在建立等式关系时，由于T_4_9存在0元素，故首先可以满足：
T_4_9 1[3,3]＝0   (11)
经推导：
T_4_9 1[3,3]＝sinθ 9(sinφ xcosφ z+cosφ xsinφ ysinφ z)-cosθ 9cosφ ysinφ z
则有
sinθ 9(sinφ xcosφ z+cosφ xsinφ ysinφ z)-cosθ 9cosφ ysinφ z＝0  (12)
其中，θ 9为串联臂第9关节臂DH参数，φ x为动坐标系绕自身X轴的欧拉角，φ z为动坐标系绕自身Z轴的欧拉角。由等式(12)可得：
当sinθ 9cosφ xsinφ y-cosθ 9cosφ y＝0时，φ z＝90°；
当sinθ 9cosφ xsinφ y-cosθ 9cosφ y≠0时，结合式子(2)、(3)和θ 9可得：
Figure PCTCN2022098189-appb-000009
在整个重建过程中，动静坐标系间Z轴方向的转动运动对应的转换矩阵记为T_rota_z：
Figure PCTCN2022098189-appb-000010
则，重建术野过程中，静平台调整至与动平台平行而动平台还未运动时动静坐标系之间的转换矩阵T_s_m_input为
Figure PCTCN2022098189-appb-000011
T_s_m_input由根据主手传递的位置信息计算得到，为已知量。根据式(15)可计算出动坐标系绕自身X、Y轴的欧拉角：
φ x＝arcsin(T_s_m_input(3,2))
φ y＝arcsin(T_s_m_input(1,3))
至此可将术野重建过程分为“位姿变换阶段”和“动坐标系回归零位阶段”。其中“位姿变换阶段”为保持动平台在机械坐标系下的位姿不动，将静平台运动至与动平台平行且Z轴共线位置的过程；“动坐标系回归零位阶段”为动平台绕自身Z轴旋转φ z使Stewart平台回到零位的过程。
重建前后，静坐标系转换矩阵、动静坐标系转换矩阵有如下关系：
T_machine_s_begin×T_s_begin_m_end＝T_machine_s_end×T_s_end_m_end  (16)
故，重建后表示静坐标系位姿的转换矩阵为：
T_machine_s_end＝T_machine_s_begin×T_s_begin_m_end×T_s_end_m_end -1  (17)
其中T_machine_s_end为重建后机械坐标系到静坐标系转换矩阵，T_s_end_m_end为动静坐标系零位转换矩阵，且
Figure PCTCN2022098189-appb-000012
至此，可明确“位姿变换阶段”的运动目标：
①.串联关节臂运动目标：T_machine_s_begin→T_machine_s_end；
②.Stewart平台运动目标：T_s_m_input→T_s_end_m_end×T_rota_z
(φ z此时取为-φ z。由于动平台绕自身Z轴转动，总角度为φ z，则相对于零位状态，转动过程中，动静坐标系间欧拉角描述应为-φ z)。
在其中一些实施例中，如图8所示，根据动平台的当前位姿和静平台的目标位姿对静平台进行位姿变换包括以下步骤：
步骤S810：获取在重建前静平台的原始位姿，对静平台的原始位姿和目标位姿进行插补，获取静平台在位姿变换过程中的中间位姿；
步骤S820：根据中间位姿和动平台的当前位姿对静平台进行位置变换和姿态变换，其中， 位置变换包括以远心不动点为圆心的圆弧运动变换和指向圆心的直线运动变换，姿态变换包括静坐标系绕机械坐标系Z轴的旋转变换和静坐标系绕自身Y轴的旋转变换。
在本实施例中，对静平台的原始位姿和目标位姿进行插补，得到静平台在位姿变换过程中的中间位姿，利用中间位姿可规划静平台回归零位时的路径，在该路径上对静平台进行位置变换和姿态变换，使静平台在运动过程中保持平滑顺畅，并保证静平台的运动精度。
进一步的，位置变换中，以远心不动点为圆心的圆弧运动变换和指向圆心的直线运动变换可同时进行；姿态变换中，静坐标系绕机械坐标系Z轴的旋转变换和静坐标系绕自身Y轴的旋转变换也可同时进行。
具体的，对“位姿变换阶段”静平台的运动进行插补及运动学解算。根据确认的运动目标，对静坐标系的运动过程进行位姿插补，如图9所示。
(1)位置插补
位置插补目标为将静坐标系从重建前所在空间位置，沿一定的轨迹由图9所示S 0运动到重建后所在目标位置S 1。首先通过运动规划，寻找该运动轨迹，然后建立以远心点为原点的坐标系，将运动轨迹曲线各点映射到该坐标系下，最后通过新建立的坐标系转换矩阵，求得运动轨迹点在机械坐标系下的坐标。
如图3所示，位置插补过程分为两个运动。运动1：以远心点为圆心，r0为半径的圆弧运动；运动2：沿半径r指向圆心方向的直线运动，即半径r0不断变化的运动。二者合成之后所形成的运动轨迹如图9静坐标系运动轨迹，为一半径从r0变化为r1的曲线运动。
其中，象征圆弧运动的变量θr以Δθ为增量，从0变化到θ，象征直线运动的变量r以Δr为变化量，从r0变化到r1以及从0变化到θ的过程中，设插值点数目为N，则第i个插值点与初始位置夹角θr以及半径r：
Figure PCTCN2022098189-appb-000013
Figure PCTCN2022098189-appb-000014
以远心点F为原点OF，
Figure PCTCN2022098189-appb-000015
向量为XF轴正向，
Figure PCTCN2022098189-appb-000016
结果为ZF轴正向，YF轴正向满足右手定则，建立如图3中所示的远心点坐标系O F-X FY FZ F，则在远心点坐标系下，第i个插值点坐标值为：
x＝r×cosθ r，y＝r×sinθ r，z＝0  (21)
记为location_pre(i)＝[r×cosθ r；r×sinθ r；0；1]
远心点坐标系O F-X FY FZ F在机械坐标系下的位姿矩阵T_machine_dm记为：
Figure PCTCN2022098189-appb-000017
其中n x，n y，n z为Xdm轴相对于全局x，y，z轴夹角的余弦值；o x，o y，o z为Ydm轴相对于全局x，y，z轴夹角的余弦值；a x，a y，a z为Zdm轴相对于全局x，y，z轴夹角的余弦值；X F，Y F，Z F为远心点在机械坐标系下的坐标。
根据前文坐标系的建立，可求得各参数如下，其中fix为重建前远心点在静坐标系下坐标，为已知量：
Figure PCTCN2022098189-appb-000018
Figure PCTCN2022098189-appb-000019
Figure PCTCN2022098189-appb-000020
Figure PCTCN2022098189-appb-000021
将式子(23)-(26)代入式(22)，即可得到全局坐标系下远心点坐标系的位姿矩阵T_machine_dm，则第i个插值点在全局坐标系下坐标值location(i)为：
location(i)＝T_machine_dm×location_pre(i)     (27)
至此，位置插补工作结束，运动轨迹中每一个插值点的全局坐标均存储在数组location中。下面将进行姿态插补。
(2)姿态插补
姿态插补目标为，将静坐标系重建前姿态按照一定的法则，变换为重建后的目标姿态，如图10所示。由于机械结构限制，在姿态变换过程中，静坐标系Y0轴需时刻与全局坐标系XY平面平行，故如图9所示，姿态变换分为2个部分：①静坐标系绕全局Z轴旋转；②静坐标系绕自身Y0轴旋转。故首先应通过重建前后静坐标系姿态关系，求得2个旋转角度，再将2个角度分别线性插值，最后一系列由各插值点组成的连续变换数组。
由于2个变换过程均满足平行条件，可同时进行。因此，姿态变换过程可描述为下式：
R_rotate_posture＝R_z×R_machine_s_begin×R_y  (28)
其中，R_rotate_posture表示第i个插值点在全局坐标系下旋转矩阵；R_machine_s_begin为重建前静坐标系旋转转换矩阵，其由转换矩阵T_machine_s_begin第1-3行、第1-3列9个元素构成；R_z表示绕全局Z轴变换过程；R_y表示绕静坐标系Y 0轴变换过程。
Figure PCTCN2022098189-appb-000022
Figure PCTCN2022098189-appb-000023
其中θ z、θ y分别表示第i个插值点与重建前静坐标系间绕全局Z轴和自身Y0轴旋转角度，由式(28)可得，重建前后姿态旋转矩阵应满足：
R＝R_machine_s_end×R_y -1＝R_z×R_machine_s_begin  (31)
其中R_machine_s_end为重建后静坐标系旋转矩阵，由转换矩阵T_machine_s_end第1-3行、第1-3列9个元素构成；
取等式(31)两侧矩阵元素R(1,2)和R(2,2)可得
Figure PCTCN2022098189-appb-000024
根据上述方程组，两式相除，即可解得由重建前状态变换至重建后状态所需转角θ z，记 为α z。
令
Figure PCTCN2022098189-appb-000025
Figure PCTCN2022098189-appb-000026
同理，取式子(31)矩阵元素R(3,1)和R(3,3)可得
Figure PCTCN2022098189-appb-000027
根据上述方程组，两式相除，即可解得由重建前状态变换至重建后状态所需转角θ y，记为α y。
令
Figure PCTCN2022098189-appb-000028
Figure PCTCN2022098189-appb-000029
在整个姿态变换过程中，θ z、θ y分别以Δθ z、Δθ y为增量，从0变化至α z、α y，插值点数目与位置插补相同，为N，则第i个插值点转角参数θ z、θ y可表示为：
Figure PCTCN2022098189-appb-000030
Figure PCTCN2022098189-appb-000031
将第i个插值点参数记录在数组posture中，可表示为：
Figure PCTCN2022098189-appb-000032
至此，姿态插补工作结束，所有插值点姿态参数记录在数组posture。
在其中一些实施例中，如图11所示，根据中间位姿和动平台的当前位姿对静平台进行位置变换和姿态变换包括以下步骤：
步骤S1110：将中间位姿转换为静坐标系至机械坐标系间的第九转换矩阵和静坐标系至动坐标系间的第十转换矩阵；静坐标系是静平台所在的坐标系，动坐标系是动平台所在的坐标系；
步骤S1120：根据第九转换矩阵和第十转换矩阵对被动臂的关节的姿态参数进行变换，其中，被动臂与静平台连接；
步骤S1130：根据第九转换矩阵和第十转换矩阵获取伸缩元件的第二伸缩量，根据第二伸缩量对伸缩元件进行调节，以使动平台在机械坐标系下的当前位姿保持不变。
在本实施例中，根据中间位姿和动平台的当前位姿对静平台进行位置变换和姿态变换，需要通过被动臂的关节的姿态参数变换和伸缩元件的伸缩调节来实现。将中间位姿转换为静坐标系至机械坐标系间的第九转换矩阵和静坐标系至动坐标系间的第十转换矩阵，利用第九转换矩阵和第十转换矩阵来获取被动臂的关节的姿态参数以及伸缩元件的第二伸缩量，进而进行中间位姿变换，使静平台在运动过程中保持平滑顺畅，并保证静平台的运动精度。
进一步的，在获得被动臂的关节的姿态参数和伸缩元件的第二伸缩量后，可进而解析被 动臂的关节的驱动参数，以及伸缩元件的驱动参数，来实现上述被动臂的关节的姿态参数和伸缩元件的第二伸缩量。
具体的，被动臂的关节的姿态参数和伸缩元件的第二伸缩量的解析可称为位姿插补运动学解算。运动学解算部分通过(1)位置插补和(2)姿态插补获得的参数转换为描述静坐标系位姿的转换矩阵和动静坐标系间转换矩阵，再分别解算至串联关节臂和Stewart平台的运动驱动上，最后获得所需各驱动参数。
由(1)位置插补和(2)姿态插补可知，第i个插值点位置描述由数组location获得，姿态描述由数组posture获得，通过式(28)可将第i个插值点转动角度转换为旋转矩阵R_rotate_posture(i)，则机械坐标系到该点静坐标系转换矩阵可表示为：
Figure PCTCN2022098189-appb-000033
此时，动静坐标系间转换矩阵可表示为：
T_s_m_i＝T_machine_s_i -1×T_machine_m_mid  (38)
其中，T_machine_m_mid＝T_machine_s_begin×T_s_m_input。
①串联关节臂驱动参数求解：
将T_s_m_input对式(4)中的T_s_begin_m_end进行替换，相应地，将T_s_m_i对式(4)中的T_s_end_m_end进行替换，依然能使等式成立，得到
Figure PCTCN2022098189-appb-000034
其中T_4_9 i表示第i个插值点第4关节坐标系到第9关节坐标系间转换矩阵。
由式(38)可得
Figure PCTCN2022098189-appb-000035
根据等式(40)两侧矩阵对应项相等，综合考虑matlab反三角函数求解取值范围，可得到串联关节臂中各待求参数表达式：
θ 7i＝arcsin[T_4_9 i(2,3)×cosθ 5-T_4_9 i(1,3)×sinθ 5]   (41)
d 8i＝-T_4_9 i(3,4)   (42)
θ 9i＝arcsin T_4_9 i(3,2)   (43)
a)若T_4_9 i(2,4)＝0，则：
θ 5i＝90°    (44)
d 6i＝a 4-T_4_9 i(1,4)    (45)
b)若T_4_9 i(2,4)≠0，则：
θ 5i＝arccos(T_4_9 i(2,4)/d 6)   (46)
Figure PCTCN2022098189-appb-000036
并将计算得到的N组DH参数存储在数组dh_update_po中。
②Stewart驱动解算过程：
根据Stewart坐标系建立，计算动铰接点在动坐标系、静铰接点在静坐标系的相对坐标。以静铰接点坐标计算为例，根据前文叙述，先将静铰接点S i(i＝1～6)由静坐标系原点O S移动到静坐标系X S轴和静坐标系平台分布圆周交点处(如图12中的(c)所示)，此时坐标值为(r s，0)。
根据平面坐标旋转定理，任意坐标点为(x，y)绕原点旋转θ角度后的坐标表示为
x′＝xcosθ-ysinθ，y′＝xsinθ+ycosθ   (48)
将坐标(r s，0)作为x，y的值带入公式(48)，则其坐标可表示为
(ss ix，ss iy)＝(r scosθ si，r ssinθ si)   (49)
其中θ si为以静坐标系原点为圆心从交点旋转到相应铰接点的旋转角度。
静平台坐标系原点定义在静铰接点所在平面中心处，因此任意静铰接点在静坐标系下的ZS轴向坐标为0，同时考虑到坐标旋转和平移变换，可得到静铰接点在静平台的齐次坐标：
S i＝(ss ix，ss iy，0，1)
如图12中的(c)所示，相应铰接点变换过程中的旋转角度由表1给出。
表1静铰接点相应变换旋转角度
Figure PCTCN2022098189-appb-000037
同理，获得动铰接点在动坐标系下的齐次坐标：
M i＝(mm ix，mm iy，0，1)
如图12中的(c)所示，相应铰接点变换过程中的旋转角度由表2给出。
表2动铰接点相应变换旋转角度
Figure PCTCN2022098189-appb-000038
由式(38)可得，此时动铰接点在静坐标系下坐标为：
Figure PCTCN2022098189-appb-000039
由三维空间两点间的距离公式，计算任意一对动、静铰接点之间的距离：
Figure PCTCN2022098189-appb-000040
将此数值与初始杆长相减，即可得到各杆驱动参数。
将计算得到的N组各杆驱动参数存储在数组L_mark_po中。
串联臂和Stewart平台分别依次读取数组dh_update_po和数组L_mark_po中的参数，即可在动平台位姿相对机械坐标系静止的情况下使静平台以确定的轨迹运动至与动平台平行的位置。至此“位姿变换阶段”完成。
在其中一些实施例中，如图13所示，根据扭转角度对动平台进行旋转变换，以使并联平台回归零位包括以下步骤：
步骤S1310：对扭转角度进行插补，获取动平台在旋转变换过程中的中间角度；
步骤S1320：根据所述中间角度和所述动平台的当前角度对所述动平台进行旋转变换。
在其中一些实施例中，如图14所示，根据所述中间角度和所述动平台的当前角度对所述动平台进行旋转变换包括以下步骤：
步骤S1410：根据中间角度获取静坐标系和动坐标系之间的第十一转换矩阵，根据第十一转换矩阵获取伸缩元件的第三伸缩量，根据第三伸缩量对伸缩元件进行调节，以使并联平台回归零位。
在本实施例中，在对动平台进行旋转变换时，也可以对扭转角度进行插补，得到动平台在旋转变换过程中的中间角度，利用中间角度可转换得到静坐标系和动坐标系之间的第十一转换矩阵，并进而得到伸缩元件的第三伸缩量，对伸缩元件进行调节后，使动平台回归零位，实现并联平台回归零位。
进一步的，可将第三伸缩量转换为伸缩元件的驱动参数，并对伸缩元件进行驱动调节。
在其中一些实施例中，如图15所示，重建器械术野中心的控制方法还包括以下步骤：
步骤S1510：在所述并联平台回归零位后，根据重建术野后所述静坐标系与所述机械坐标系之间的转换关系，获取所述远心不动点在重建术野后所述静坐标系下的坐标，其中，所述远心不动点在所述机械坐标系下的坐标不变。
在本实施例中，可重新确认远心不动点在重建术野后所述静坐标系下的坐标，以便于手术位置的确认。
具体的，经过“位姿变换阶段”，静坐标系位姿已满足重建后的位姿要求，Stewart平台动坐标系相对于静坐标系间还存在一绕动坐标系Z轴的转动，故该阶段串联臂保持不动，Stewart平台对前文描述的绕自身Z轴转动角度进行插值并解算Stewart相应驱动参数。
目标为：T_s_m_rota→T_s_end_m_end。对“动坐标系回归零位阶段”动平台的运动进行插补及运动学解算如下。
该阶段运动为：动平台绕自身Z轴转动，总角度为φ z，则相对于零位状态，转动过程中，动静坐标系间欧拉角描述应为-φ z。
设插值点数目为M，则第i个插补点的动静坐标系间欧拉角描述eula_z应为：
Figure PCTCN2022098189-appb-000041
第i个插补点的动静坐标系间转换矩阵T_s_m_rota_z_i为：
Figure PCTCN2022098189-appb-000042
根据上述2○Stewart驱动解算过程的方法，用T_s_m_rota_z_i替换其中的T_s_m_i，再用同样的方法求得M组各驱动杆参数，并存入数组L_mark_rota_z，与L_mark_po共同组成整个重建过程stewart平台的驱动L_mark_re_update：
L_mark_re_update＝[L_mark_po；L_mark_rota_z]  (54)
重建过程串联关节臂的DH参数即为位姿插补时的参数：
dh_re_update＝dh_update_po   (55)
Stewart平台完成“位姿变换阶段”后，依次读取数组L_mark_rota_z中的数组参数，即可使动坐标系绕自身Z轴旋转φ z，使Stewart回到零位。至此“动坐标系回归零位阶段”完成， 即整个术野重建过程完成。
另外，为使手术正常进行，还需要重新获得静坐标系下远心不动点的坐标并输入给系统。术野重建过程中，静平台位姿改变，但远心不动点在机械坐标系下坐标不变，根据重建术野后得到的静坐标与机械坐标的转换关系，可得到远心不动点在重建术野后静坐标系下的坐标，记为fix_new:
fix_new＝T_machine_s_end -1×T_machine_s_begin×fix   (56)
在一个具体实施例中，重建器械术野中心的控制步骤如下：
1.位姿变换阶段：
①通过被动臂的关节信息，解算重建前静坐标系在机械坐标系下的位姿转换矩阵
T_machine_s_begin。
②根据动平台在机械坐标系下的位姿信息，解算重建后静坐标系在机械坐标系下的位姿转换矩阵T_machine_s_end。由于被动臂只有5个自由度，仅通过被动臂的运动无法完全实现Stewart姿态回零，因此这里解算得到的静坐标系与动坐标系之间还可能存在一个Z轴方向的扭转。
③根据术野重建前后的静坐标系位姿矩阵，对中间轨迹和姿态进行位置插补和姿态插补，得到一系列中间过程中静坐标系在机械坐标系下的位姿矩阵T_machine_s，并通过被动臂逆运动学解算每个插补点之间被动臂5个关节的运动信息dh_update_po，使静坐标系根据插补点轨迹运动至重建后的预设位姿。
④在步骤③进行的同一时间内，即在插补过程中，根据动平台在机械坐标系下的位姿信息，以及静平台插补位姿在机械坐标系下的姿态矩阵，得到插补点动静坐标系间转换矩阵T_s_m，根据Stewart平台逆运动学解算Stewart平台六个驱动杆的长度L_mark_po，使静平台运动的过程中动平台在机械坐标系下的位姿保持静止。
2.动坐标系回归零位阶段：
根据位姿变换阶段第②点中所解释的，位姿变换阶段无法使Stewart平台完全回到零位，动静平台之间还可能存在一个Z轴方向的扭转，由于被动臂的自由度限制已无法通过调整静平台的姿态来使Stewart平台回到零位，故而选择控制动平台绕Z轴转动来使Stewart平台回到零位。动平台的转动会导致器械绕Z轴转动，但这并不会违背远心不动点原则，也不会改变器械的术野中心，可以有效地完成术野中心重建。
根据位姿变换阶段完成后动静坐标系在机械坐标系下的位姿矩阵，计算动静之间的扭转角度，对该角度进行多点插值，再根据Stewart平台逆运动学解算每个差值点Stewart平台六个驱动杆的长度L_mark_po，使动平台以相应的速度绕Z轴旋转，直至Stewart平台回到零点位置。
本实施例还提供一种机械臂的控制方法，其中，机械臂包括并联平台，并联平台包括动平台、静平台和连接动平台和静平台的伸缩元件，如图16所示，该方法包括以下步骤：
步骤S1610：获取动平台在机械坐标系下的当前位姿，其中，机械坐标系是动平台所在机械臂的全局坐标系；
步骤S1620：根据动平台的当前位姿，获取静平台在机械坐标系下的目标位姿；以及
步骤S1630：根据动平台的当前位姿和静平台的目标位姿，对静平台进行位姿变换。
作为示例，静平台在机械坐标系下的目标位姿可以为：静平台的目标位姿的中心在动平台的当前位姿的中心轴线上，且处于目标位姿的静平台与动平台平行。
作为示例，机械臂还包括被动臂，被动臂与静平台连接。如图17所示，根据动平台的当前位姿和静平台的目标位姿，对静平台进行位姿变换包括以下步骤：
步骤S1710：根据动平台的当前位姿和静平台的目标位姿，确定控制被动臂的驱动参数；以及
步骤S1720：基于驱动参数，通过控制被动臂，对静平台进行位姿变换。
作为示例，被动臂包括多个串联的关节，如图18所示，根据动平台的当前位姿和静平台 的目标位姿，对静平台进行位姿变换的步骤包括：步骤S1810：根据动平台的当前位姿和静平台的目标位姿，通过被动臂对静平台进行位姿变换，其中，在对被动臂的关节的姿态参数进行变换时，多个串联的关节中的部分关节的姿态参数不变，改变多个串联的关节中的另一部分关节的姿态参数。
作为示例，被动臂包括串联的第一关节、第二关节和第三关节，其中，第一关节和第三关节的姿态参数不变，第三关节与静平台连接。
作为示例，对静平台进行位姿变换还包括：在静平台位姿变换过程中，动平台在机械坐标系下的当前位姿保持不变。
作为示例，对静平台进行位姿变换还包括：根据动平台的当前位姿和静平台的目标位姿获取伸缩元件的第一伸缩量，在对静平台进行位姿变换时，根据第一伸缩量对伸缩元件进行调节，以使动平台在机械坐标系下的当前位姿保持不变。
作为示例，对静平台进行位姿变换包括以下步骤：获取静平台的原始位姿，对静平台的原始位姿和目标位姿进行插补，获取静平台在位姿变换过程中的中间位姿；以及根据中间位姿和动平台的当前位姿对静平台进行位置变换和姿态变换。
作为示例，位置变换包括以远心不动点为圆心的圆弧运动变换和指向圆心的直线运动变换，姿态变换包括静坐标系绕机械坐标系Z轴的旋转变换和静坐标系绕自身Y轴的旋转变换。
作为示例，位置变换和姿态变换可以同时进行。
作为示例，该机械臂的控制方法还包括以下步骤：根据动平台的当前位姿和静平台的目标位姿，获取动平台和静平台之间在中心轴线上的扭转角度，根据扭转角度对动平台进行旋转变换，以使并联平台回归零位。
作为示例，动平台的当前位姿为：在重建器械术野中心后，动平台在机械坐标系下的位姿；静平台的目标位姿为：在重建器械术野中心后，静平台在机械坐标系下的位姿。
本实施例还提供了一种重建器械术野中心的控制系统，该系统用于实现上述实施例及优选实施方式，已经进行过说明的不再赘述。如以下所使用的，术语“模块”等可以实现预定功能的软件和/或硬件的组合。尽管以下实施例所描述的系统较佳地以软件来实现，但是硬件，或者软件和硬件的组合的实现也是可能并被构想的。
图19是根据本公开实施例的重建器械术野中心的控制系统的结构框图，如图19所示，该系统包括：
第一获取模块210，用于获取重建器械术野中心后，动平台在机械坐标系下的当前位姿，其中，机械坐标系是动平台所在机械臂的全局坐标系；
第二获取模块220，用于根据动平台的当前位姿获取重建器械术野中心后，静平台在机械坐标系下的目标位姿，其中，动平台和静平台是机械臂中通过伸缩元件连接的并联平台，静平台的目标位姿的中心在动平台的当前位姿的中心轴线上，且处于目标位姿的静平台与动平台平行；
第一变换模块230，用于根据动平台的当前位姿和静平台的目标位姿对静平台进行位姿变换；
第二变换模块240，用于根据动平台的当前位姿和静平台的目标位姿获取动平台和静平台之间在中心轴线上的扭转角度，根据扭转角度对动平台进行旋转变换，以使并联平台回归零位。
在其中一些实施例中，第一变换模块230还用于根据动平台的当前位姿和静平台的目标位姿获取经重建并回归零位状态时，静坐标系到动坐标系间的第一转换矩阵；静坐标系是静平台所在的坐标系，动坐标系是动平台所在的坐标系；获取经重建但未回归零位状态时，静坐标系到动坐标系间的第二转换矩阵；以及重建前机械坐标系到静坐标系间的第三转换矩阵；根据第一转换矩阵、第二转换矩阵和第三转换矩阵获取重建后机械坐标系到静坐标系间的第四转换矩阵；根据第四转换矩阵获取机械臂中被动臂的关节坐标系间的第五转换矩阵，根据第五转换矩阵对被动臂的关节的姿态参数进行变换，其中，被动臂与静平台连接。
在其中一些实施例中，被动臂包括串联的第一关节、第二关节和第三关节，其中，第一关节和第三关节的姿态参数不变，第三关节与静平台连接；
第一变换模块230还用于获取机械坐标系到第一关节的坐标系的第六转换矩阵，以及第二关节的坐标系到第三关节的坐标系间的第七转换矩阵；根据第五转换矩阵、第六转换矩阵和第七转换矩阵获取第一关节到第二关节间的第八转换矩阵；根据第八转换矩阵对第二关节的姿态参数进行变换。
在其中一些实施例中，第一变换模块230还用于根据动平台的当前位姿和静平台的目标位姿获取伸缩元件的第一伸缩量，在对静平台进行位姿变换时，根据第一伸缩量对伸缩元件进行调节，以使动平台在机械坐标系下的当前位姿保持不变。
在其中一些实施例中，第一变换模块230还用于获取在重建前静平台的原始位姿，对静平台的原始位姿和目标位姿进行插补，获取静平台在位姿变换过程中的中间位姿；根据中间位姿和动平台的当前位姿对静平台进行位置变换和姿态变换，其中，位置变换包括以远心不动点为圆心的圆弧运动变换和指向圆心的直线运动变换，姿态变换包括静坐标系绕机械坐标系Z轴的旋转变换和静坐标系绕自身Y轴的旋转变换。
在其中一些实施例中，第一变换模块230还用于将中间位姿转换为静坐标系至机械坐标系间的第九转换矩阵和静坐标系至动坐标系间的第十转换矩阵；静坐标系是静平台所在的坐标系，动坐标系是动平台所在的坐标系；根据第九转换矩阵和第十转换矩阵对被动臂的关节的姿态参数进行变换，其中，被动臂与静平台连接；根据第九转换矩阵和第十转换矩阵获取伸缩元件的第二伸缩量，根据第二伸缩量对伸缩元件进行调节，以使动平台在机械坐标系下的当前位姿保持不变。
在其中一些实施例中，第二变换模块240还用于对扭转角度进行插补，获取动平台在旋转变换过程中的中间角度；根据中间角度获取静坐标系和动坐标系之间的第十一转换矩阵，根据第十一转换矩阵获取伸缩元件的第三伸缩量，根据第三伸缩量对伸缩元件进行调节，以使并联平台回归零位。
本公开实施例还提供了一种机械臂的控制系统，其中，机械臂包括并联平台，并联平台包括动平台、静平台和连接动平台和静平台的伸缩元件，如图20所示，该系统可以包括：
第一获取模块410，用于获取动平台在机械坐标系下的当前位姿，其中，机械坐标系是动平台所在机械臂的全局坐标系；
第二获取模块420，用于根据动平台的当前位姿，获取静平台在机械坐标系下的目标位姿；以及
第一变换模块430，用于根据动平台的当前位姿和静平台的目标位姿，对静平台进行位姿变换。
在其中一些实施例中，机械臂还包括被动臂，被动臂与静平台连接，其中，第一变换模块430还用于：根据所述动平台的当前位姿和所述静平台的目标位姿，确定控制所述被动臂的驱动参数；以及基于所述驱动参数，通过控制所述被动臂，对所述静平台进行位姿变换。
在其中一些实施例中，机械臂还包括被动臂，被动臂与静平台连接，被动臂包括多个串联的关节，在对被动臂的关节的姿态参数进行变换时，多个串联的关节中的部分关节的姿态参数不变，改变多个串联的关节中的另一部分关节的姿态参数。
在其中一些实施例中，被动臂包括串联的第一关节、第二关节和第三关节，其中，第一关节和第三关节的姿态参数不变，第三关节与静平台连接。
在其中一些实施例中，在静平台位姿变换过程中，动平台在机械坐标系下的当前位姿保持不变。
在其中一些实施例中，第一变换模块430还用于：根据动平台的当前位姿和静平台的目标位姿获取伸缩元件的第一伸缩量，在对静平台进行位姿变换时，根据第一伸缩量对伸缩元件进行调节，以使动平台在机械坐标系下的当前位姿保持不变。
在其中一些实施例中，第一变换模块430还用于：获取静平台的原始位姿，对静平台的 原始位姿和目标位姿进行插补，获取静平台在位姿变换过程中的中间位姿；以及根据中间位姿和动平台的当前位姿对静平台进行位置变换和姿态变换。
在其中一些实施例中，位置变换包括以远心不动点为圆心的圆弧运动变换和指向圆心的直线运动变换，姿态变换包括静坐标系绕机械坐标系Z轴的旋转变换和静坐标系绕自身Y轴的旋转变换。
在其中一些实施例中，位置变换和姿态变换同时进行。
在其中一些实施例中，静平台的目标位姿为：静平台的目标位姿的中心在动平台的当前位姿的中心轴线上，且处于目标位姿的静平台与动平台平行。
在其中一些实施例中，该控制系统还包括：第二变换模块440，用于根据动平台的当前位姿和静平台的目标位姿，获取动平台和静平台之间在中心轴线上的扭转角度，根据扭转角度对动平台进行旋转变换，以使并联平台回归零位。
如图21所示，本公开实施例还提供了一种重建器械术野中心的控制设备300，包括机械臂310和控制装置320；
控制装置320用于根据上述第一方面的重建器械术野中心的控制方法对机械臂310上的静平台进行位姿变换，对机械臂310上的动平台进行旋转变换，或者根据上述第二方面的机械臂的控制方法对机械臂310上的静平台进行位姿变换。
上述重建器械术野中心的控制设备300，通过控制装置320对机械臂310上的静平台进行位姿变换，对机械臂310上的动平台进行旋转变换，实现了并联平台回归零位，解决了相关技术中存在的并联机器人难以实现姿态回零的问题，提升了重建器械术野中心的控制准确度，保证并联平台的运动性能。
如图22所示，本公开实施例还提供了一种机械臂组件500，机械臂组件包括：机械臂510；以及控制装置520，控制装置用于：根据上述重建器械术野中心的控制方法对机械臂上的静平台进行位姿变换，对机械臂上的动平台进行旋转变换，或者根据上述的机械臂的控制方法对机械臂上的静平台进行位姿变换。
如图23所示，本公开实施例还提供了一种手术机器人，手术机器人包括：底座610；以及上述重建器械术野中心的控制设备或者上述机械臂组件。
本公开实施例还提供了一种可读存储介质，其上存储有可执行程序，该可执行程序被处理器执行时实现如上述重建器械术野中心的控制方法或者实现上述机械臂的控制方法。
上述可读存储介质，通过在处理器上运行可执行程序，可以实现并联平台回归零位，解决了相关技术中存在的并联机器人难以实现姿态回零的问题，提升了重建器械术野中心的控制准确度，保证并联平台的运动性能。
本公开实施例还可提供一种计算机程序产品，该计算机程序产品包括计算机指令，计算机指令被处理器执行时实现如上述的重建器械术野中心的控制方法或者实现如上述的机械臂的控制方法。
以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成。所述的程序可以存储于可读取存储介质中。该程序在执行时，包括上述方法所述的步骤。所述的存储介质，包括：ROM/RAM、磁碟、光盘等。
以上所述实施例仅表达了本公开的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本公开构思的前提下，还可以做出若干变形和改进，这些都属于本公开的保护范围。因此，本公开专利的保护范围应以所附权利要求为准。

Claims (47)

  1. 一种重建器械术野中心的控制方法，其中，所述方法包括以下步骤：
    获取重建器械术野中心后，动平台在机械坐标系下的当前位姿，其中，所述机械坐标系是所述动平台所在机械臂的全局坐标系；
    根据所述动平台的当前位姿获取重建器械术野中心后，静平台在所述机械坐标系下的目标位姿，其中，所述动平台和所述静平台是所述机械臂中通过伸缩元件连接的并联平台，所述静平台的目标位姿的中心在所述动平台的当前位姿的中心轴线上，且处于目标位姿的静平台与所述动平台平行；
    根据所述动平台的当前位姿和所述静平台的目标位姿对所述静平台进行位姿变换；以及
    根据所述动平台的当前位姿和所述静平台的目标位姿获取所述动平台和所述静平台之间在所述中心轴线上的扭转角度，根据所述扭转角度对所述动平台进行旋转变换，以使所述并联平台回归零位。
  2. 根据权利要求1所述的重建器械术野中心的控制方法，其中，所述根据所述动平台的当前位姿和所述静平台的目标位姿对所述静平台进行位姿变换包括以下步骤：
    根据所述动平台的当前位姿和静平台的目标位姿获取经重建并回归零位状态时，静坐标系到动坐标系间的第一转换矩阵；所述静坐标系是所述静平台所在的坐标系，所述动坐标系是所述动平台所在的坐标系；
    获取经重建但未回归零位状态时，所述静坐标系到所述动坐标系间的第二转换矩阵；以及重建前所述机械坐标系到所述静坐标系间的第三转换矩阵；
    根据所述第一转换矩阵、所述第二转换矩阵和所述第三转换矩阵获取重建后所述机械坐标系到所述静坐标系间的第四转换矩阵；以及
    根据所述第四转换矩阵获取所述机械臂中被动臂的关节坐标系间的第五转换矩阵，根据所述第五转换矩阵对所述被动臂的关节的驱动参数进行变换，其中，所述被动臂与所述静平台连接。
  3. 根据权利要求2所述的重建器械术野中心的控制方法，其中，所述被动臂包括串联的第一关节、第二关节和第三关节，其中，所述第一关节和第三关节的驱动参数不变，所述第三关节与所述静平台连接；
    所述根据所述第五转换矩阵对所述被动臂的关节的驱动参数进行变换包括以下步骤：
    获取所述机械坐标系到所述第一关节的坐标系的第六转换矩阵，以及所述第二关节的坐标系到所述第三关节的坐标系间的第七转换矩阵；
    根据所述第五转换矩阵、所述第六转换矩阵和所述第七转换矩阵获取所述第一关节到所述第二关节间的第八转换矩阵；以及
    根据所述第八转换矩阵对所述第二关节的驱动参数进行变换。
  4. 根据权利要求3所述的重建器械术野中心的控制方法，其中，所述第二关节包括依次连接的第一转动副、第一移动副、第二转动副、第二移动副和第三转动副，所述第一转动副与所述第一关节转动连接，所述第三转动副与所述第三关节转动连接；
    所述根据第八转换矩阵对所述第二关节的驱动参数进行变换包括以下步骤：
    获取所述第一关节到所述第三转动副的驱动参数变换关系，根据所述第八转换矩阵和所述驱动参数变换关系，求解得到所述第一转动副、所述第一移动副、所述第二转动副、所述第二移动副和所述第三转动副的驱动参数。
  5. 根据权利要求1所述的重建器械术野中心的控制方法，其中，所述根据所述动平台的当前位姿和所述静平台的目标位姿对所述静平台进行位姿变换包括以下步骤：
    根据所述动平台的当前位姿和所述静平台的目标位姿获取所述伸缩元件的第一伸缩量，在对所述静平台进行位姿变换时，根据所述第一伸缩量对所述伸缩元件进行调节，以使所述动平台在所述机械坐标系下的当前位姿保持不变。
  6. 根据权利要求1至5中任意一项所述的重建器械术野中心的控制方法，其中，所述根据所述动平台的当前位姿和所述静平台的目标位姿对所述静平台进行位姿变换包括以下步骤：
    获取在重建前所述静平台的原始位姿，对所述静平台的原始位姿和目标位姿进行插补，获取所述静平台在位姿变换过程中的中间位姿；以及
    根据所述中间位姿和所述动平台的当前位姿对所述静平台进行位置变换和姿态变换，其中，所述位置变换包括以远心不动点为圆心的圆弧运动变换和指向所述圆心的直线运动变换，所述姿态变换包括静坐标系绕所述机械坐标系Z轴的旋转变换和所述静坐标系绕自身Y轴的旋转变换。
  7. 根据权利要求6所述的重建器械术野中心的控制方法，其中，所述根据所述中间位姿和所述动平台的当前位姿对所述静平台进行位置变换和姿态变换包括以下步骤：
    将所述中间位姿转换为静坐标系至所述机械坐标系间的第九转换矩阵和所述静坐标系至动坐标系间的第十转换矩阵；所述静坐标系是所述静平台所在的坐标系，所述动坐标系是所述动平台所在的坐标系；
    根据所述第九转换矩阵和所述第十转换矩阵对被动臂的关节的姿态参数进行变换，其中，所述被动臂与所述静平台连接；以及
    根据所述第九转换矩阵和所述第十转换矩阵获取所述伸缩元件的第二伸缩量，根据所述第二伸缩量对所述伸缩元件进行调节，以使所述动平台在所述机械坐标系下的当前位姿保持不变。
  8. 根据权利要求7所述的重建器械术野中心的控制方法，其中，所述根据所述扭转角度对所述动平台进行旋转变换，以使所述并联平台回归零位包括以下步骤：
    对所述扭转角度进行插补，获取所述动平台在旋转变换过程中的中间角度；以及
    根据所述中间角度和所述动平台的当前角度对所述动平台进行旋转变换。
  9. 根据权利要求8所述的重建器械术野中心的控制方法，其中，所述根据所述中间角度和所述动平台的当前角度对所述动平台进行旋转变换包括以下步骤：
    根据所述中间角度获取所述静坐标系和所述动坐标系之间的第十一转换矩阵，根据所述第十一转换矩阵获取所述伸缩元件的第三伸缩量，根据所述第三伸缩量对所述伸缩元件进行调节，以使所述并联平台回归零位。
  10. 根据权利要求9所述的重建器械术野中心的控制方法，其中，还包括以下步骤：
    在所述并联平台回归零位后，根据重建术野后所述静坐标系与所述机械坐标系之间的转换关系，获取所述远心不动点在重建术野后所述静坐标系下的坐标，其中，所述远心不动点在所述机械坐标系下的坐标不变。
  11. 一种机械臂的控制方法，其中，所述机械臂包括并联平台，所述并联平台包括动平台、静平台和连接所述动平台和所述静平台的伸缩元件，所述方法包括以下步骤：
    获取所述动平台在机械坐标系下的当前位姿，其中，所述机械坐标系是所述动平台所在所述机械臂的全局坐标系；
    根据所述动平台的当前位姿，获取所述静平台在所述机械坐标系下的目标位姿，以及
    根据所述动平台的当前位姿和所述静平台的目标位姿，对所述静平台进行位姿变换。
  12. 根据权利要求11所述的方法，其中，所述机械臂还包括被动臂，所述被动臂与所述静平台连接，
    其中，所述根据所述动平台的当前位姿和所述静平台的目标位姿，对所述静平台进行位姿变换包括以下步骤：
    根据所述动平台的当前位姿和所述静平台的目标位姿，确定控制所述被动臂的驱动参数；以及
    基于所述驱动参数，通过控制所述被动臂，对所述静平台进行位姿变换。
  13. 根据权利要求11所述的方法，其中，所述机械臂还包括被动臂，所述被动臂与所述静平台连接，所述被动臂包括多个串联的关节，
    其中，所述根据所述动平台的当前位姿和所述静平台的目标位姿，对所述静平台进行位姿变换包括：根据所述动平台的当前位姿和所述静平台的目标位姿，通过所述被动臂对所述 静平台进行位姿变换，
    其中，在对所述被动臂的关节的姿态参数进行变换时，所述多个串联的关节中的部分关节的姿态参数不变，改变所述多个串联的关节中的另一部分关节的姿态参数。
  14. 根据权利要求13所述的方法，其中，所述被动臂包括串联的第一关节、第二关节和第三关节，其中，所述第一关节和所述第三关节的姿态参数不变，所述第三关节与所述静平台连接。
  15. 根据权利要求11所述的方法，其中，所述对所述静平台进行位姿变换还包括：在所述静平台位姿变换过程中，所述动平台在所述机械坐标系下的当前位姿保持不变。
  16. 根据权利要求15所述的方法，其中，所述对所述静平台进行位姿变换还包括：
    根据所述动平台的当前位姿和所述静平台的目标位姿获取所述伸缩元件的第一伸缩量，在对所述静平台进行位姿变换时，根据所述第一伸缩量对所述伸缩元件进行调节，以使所述动平台在所述机械坐标系下的当前位姿保持不变。
  17. 根据权利要求11至16中的任意一项所述的方法，其中，所述对所述静平台进行位姿变换包括以下步骤：
    获取所述静平台的原始位姿，对所述静平台的原始位姿和目标位姿进行插补，获取所述静平台在位姿变换过程中的中间位姿；以及
    根据所述中间位姿和所述动平台的当前位姿对所述静平台进行位置变换和姿态变换。
  18. 根据权利要求17所述的方法，其中，所述位置变换包括以远心不动点为圆心的圆弧运动变换和指向所述圆心的直线运动变换，所述姿态变换包括静坐标系绕所述机械坐标系Z轴的旋转变换和所述静坐标系绕自身Y轴的旋转变换。
  19. 根据权利要求17所述的方法，其中，所述位置变换和所述姿态变换同时进行。
  20. 根据权利要求11所述的方法，其中，所述目标位姿为：所述静平台的目标位姿的中心在所述动平台的当前位姿的中心轴线上，且处于目标位姿的静平台与所述动平台平行。
  21. 根据权利要求20所述的方法，其中，所述方法还包括以下步骤：
    根据所述动平台的当前位姿和所述静平台的目标位姿，获取所述动平台和所述静平台之间在所述中心轴线上的扭转角度，根据所述扭转角度对所述动平台进行旋转变换，以使所述并联平台回归零位。
  22. 一种机械臂的控制方法，其中，所述方法包括以下步骤：
    获取动平台在机械坐标系下的当前位姿，其中，所述机械坐标系是所述动平台所在机械臂的全局坐标系；
    根据所述动平台的当前位姿获取静平台在所述机械坐标系下的目标位姿，其中，所述动平台和所述静平台是所述机械臂中通过伸缩元件连接的并联平台；以及
    根据所述动平台的当前位姿和所述静平台的目标位姿对所述静平台进行位姿变换。
  23. 根据权利要求22所述的方法，其中，所述方法还包括以下步骤：
    根据所述动平台的当前位姿和所述静平台的目标位姿获取所述动平台和所述静平台之间在所述中心轴线上的扭转角度，根据所述扭转角度对所述动平台进行旋转变换，以使所述并联平台回归零位。
  24. 根据权利要求23所述的方法，其中，所述目标位姿为：所述静平台的目标位姿的中心在所述动平台的当前位姿的中心轴线上，且处于目标位姿的静平台与所述动平台平行。
  25. 根据权利要求24所述的方法，其中，所述动平台的当前位姿为：在重建器械术野中心后，所述动平台在所述机械坐标系下的位姿；所述静平台的目标位姿为：在重建器械术野中心后，所述静平台在所述机械坐标系下的位姿。
  26. 根据权利要求25所述的方法，其中，所述根据所述动平台的当前位姿和所述静平台的目标位姿对所述静平台进行位姿变换包括以下步骤：
    根据所述动平台的当前位姿和静平台的目标位姿获取经重建并回归零位状态时，静坐标系到动坐标系间的第一转换矩阵；所述静坐标系是所述静平台所在的坐标系，所述动坐标系 是所述动平台所在的坐标系；
    获取经重建但未回归零位状态时，所述静坐标系到所述动坐标系间的第二转换矩阵；以及重建前所述机械坐标系到所述静坐标系间的第三转换矩阵；
    根据所述第一转换矩阵、所述第二转换矩阵和所述第三转换矩阵获取重建后所述机械坐标系到所述静坐标系间的第四转换矩阵；以及
    根据所述第四转换矩阵获取所述机械臂中被动臂的关节坐标系间的第五转换矩阵，根据所述第五转换矩阵对所述被动臂的关节的驱动参数进行变换，其中，所述被动臂与所述静平台连接。
  27. 根据权利要求26所述的方法，其中，所述被动臂包括串联的第一关节、第二关节和第三关节，其中，所述第一关节和第三关节的驱动参数不变，所述第三关节与所述静平台连接；
    所述根据所述第五转换矩阵对所述被动臂的关节的驱动参数进行变换包括以下步骤：
    获取所述机械坐标系到所述第一关节的坐标系的第六转换矩阵，以及所述第二关节的坐标系到所述第三关节的坐标系间的第七转换矩阵；
    根据所述第五转换矩阵、所述第六转换矩阵和所述第七转换矩阵获取所述第一关节到所述第二关节间的第八转换矩阵；以及
    根据所述第八转换矩阵对所述第二关节的驱动参数进行变换。
  28. 根据权利要求27所述的方法，其中，所述第二关节包括依次连接的第一转动副、第一移动副、第二转动副、第二移动副和第三转动副，所述第一转动副与所述第一关节转动连接，所述第三转动副与所述第三关节转动连接；
    所述根据第八转换矩阵对所述第二关节的驱动参数进行变换包括以下步骤：
    获取所述第一关节到所述第三转动副的驱动参数变换关系，根据所述第八转换矩阵和所述驱动参数变换关系，求解得到所述第一转动副、所述第一移动副、所述第二转动副、所述第二移动副和所述第三转动副的驱动参数。
  29. 根据权利要求22所述的方法，其中，所述根据所述动平台的当前位姿和所述静平台的目标位姿对所述静平台进行位姿变换包括以下步骤：
    根据所述动平台的当前位姿和所述静平台的目标位姿获取所述伸缩元件的第一伸缩量，在对所述静平台进行位姿变换时，根据所述第一伸缩量对所述伸缩元件进行调节，以使所述动平台在所述机械坐标系下的当前位姿保持不变。
  30. 根据权利要求25至29中任意一项所述的方法，其中，所述根据所述动平台的当前位姿和所述静平台的目标位姿对所述静平台进行位姿变换包括以下步骤：
    获取在重建前所述静平台的原始位姿，对所述静平台的原始位姿和目标位姿进行插补，获取所述静平台在位姿变换过程中的中间位姿；以及
    根据所述中间位姿和所述动平台的当前位姿对所述静平台进行位置变换和姿态变换，其中，所述位置变换包括以远心不动点为圆心的圆弧运动变换和指向所述圆心的直线运动变换，所述姿态变换包括静坐标系绕所述机械坐标系Z轴的旋转变换和所述静坐标系绕自身Y轴的旋转变换。
  31. 根据权利要求30所述的方法，其中，所述根据所述中间位姿和所述动平台的当前位姿对所述静平台进行位置变换和姿态变换包括以下步骤：
    将所述中间位姿转换为静坐标系至所述机械坐标系间的第九转换矩阵和所述静坐标系至动坐标系间的第十转换矩阵；所述静坐标系是所述静平台所在的坐标系，所述动坐标系是所述动平台所在的坐标系；
    根据所述第九转换矩阵和所述第十转换矩阵对被动臂的关节的姿态参数进行变换，其中，所述被动臂与所述静平台连接；以及
    根据所述第九转换矩阵和所述第十转换矩阵获取所述伸缩元件的第二伸缩量，根据所述第二伸缩量对所述伸缩元件进行调节，以使所述动平台在所述机械坐标系下的当前位姿保持 不变。
  32. 根据权利要求31所述的方法，其中，所述根据所述扭转角度对所述动平台进行旋转变换，以使所述并联平台回归零位包括以下步骤：
    对所述扭转角度进行插补，获取所述动平台在旋转变换过程中的中间角度；以及
    根据所述中间角度和所述动平台的当前角度对所述动平台进行旋转变换。
  33. 根据权利要求32所述的方法，其中，所述根据所述中间角度和所述动平台的当前角度对所述动平台进行旋转变换包括以下步骤：
    根据所述中间角度获取所述静坐标系和所述动坐标系之间的第十一转换矩阵，根据所述第十一转换矩阵获取所述伸缩元件的第三伸缩量，根据所述第三伸缩量对所述伸缩元件进行调节，以使所述并联平台回归零位。
  34. 根据权利要求33所述的方法，其中，还包括以下步骤：
    在所述并联平台回归零位后，根据重建术野后所述静坐标系与所述机械坐标系之间的转换关系，获取所述远心不动点在重建术野后所述静坐标系下的坐标，其中，所述远心不动点在所述机械坐标系下的坐标不变。
  35. 根据权利要求22所述的方法，其中，所述机械臂还包括被动臂，所述被动臂与所述静平台连接，
    其中，所述根据所述动平台的当前位姿和所述静平台的目标位姿，对所述静平台进行位姿变换包括以下步骤：
    根据所述动平台的当前位姿和所述静平台的目标位姿，确定控制所述被动臂的驱动参数；以及
    基于所述驱动参数，通过控制所述被动臂，对所述静平台进行位姿变换。
  36. 根据权利要求22所述的方法，其中，所述机械臂还包括被动臂，所述被动臂与所述静平台连接，所述被动臂包括多个串联的关节，
    其中，所述根据所述动平台的当前位姿和所述静平台的目标位姿，对所述静平台进行位姿变换包括：根据所述动平台的当前位姿和所述静平台的目标位姿，通过所述被动臂对所述静平台进行位姿变换，
    其中，在对所述被动臂的关节的姿态参数进行变换时，所述多个串联的关节中的部分关节的姿态参数不变，改变所述多个串联的关节中的另一部分关节的姿态参数。
  37. 根据权利要求36所述的方法，其中，所述被动臂包括串联的第一关节、第二关节和第三关节，其中，所述第一关节和所述第三关节的姿态参数不变，所述第三关节与所述静平台连接。
  38. 根据权利要求22所述的方法，其中，所述对所述静平台进行位姿变换还包括：在所述静平台位姿变换过程中，所述动平台在所述机械坐标系下的当前位姿保持不变。
  39. 根据权利要求22、35至38中的任意一项所述的方法，其中，所述对所述静平台进行位姿变换包括以下步骤：
    获取所述静平台的原始位姿，对所述静平台的原始位姿和目标位姿进行插补，获取所述静平台在位姿变换过程中的中间位姿；以及
    根据所述中间位姿和所述动平台的当前位姿对所述静平台进行位置变换和姿态变换。
  40. 根据权利要求39所述的方法，其中，所述位置变换包括以远心不动点为圆心的圆弧运动变换和指向所述圆心的直线运动变换，所述姿态变换包括静坐标系绕所述机械坐标系Z轴的旋转变换和所述静坐标系绕自身Y轴的旋转变换。
  41. 根据权利要求39所述的方法，其中，所述位置变换和所述姿态变换同时进行。
  42. 一种重建器械术野中心的控制系统，其中，所述系统包括：
    第一获取模块，用于获取重建器械术野中心后，动平台在机械坐标系下的当前位姿，其中，所述机械坐标系是所述动平台所在机械臂的全局坐标系；
    第二获取模块，用于根据所述动平台的当前位姿获取重建器械术野中心后，静平台在所 述机械坐标系下的目标位姿，其中，所述动平台和所述静平台是所述机械臂中通过伸缩元件连接的并联平台，所述静平台的目标位姿的中心在所述动平台的当前位姿的中心轴线上，且处于目标位姿的静平台与所述动平台平行；
    第一变换模块，用于根据所述动平台的当前位姿和所述静平台的目标位姿对所述静平台进行位姿变换；以及
    第二变换模块，用于根据所述动平台的当前位姿和所述静平台的目标位姿获取所述动平台和所述静平台之间在所述中心轴线上的扭转角度，根据所述扭转角度对所述动平台进行旋转变换，以使所述并联平台回归零位。
  43. 一种机械臂的控制系统，其中，所述机械臂包括并联平台，所述并联平台包括动平台、静平台和连接所述动平台和所述静平台的伸缩元件，所述系统包括：
    第一获取模块，用于获取所述动平台在机械坐标系下的当前位姿，其中，所述机械坐标系是所述动平台所在所述机械臂的全局坐标系；
    第二获取模块，用于根据所述动平台的当前位姿，获取所述静平台在所述机械坐标系下的目标位姿；以及
    第一变换模块，用于根据所述动平台的当前位姿和所述静平台的目标位姿，对所述静平台进行位姿变换。
  44. 一种机械臂组件，其中，所述机械臂组件包括：
    机械臂；以及
    控制装置，所述控制装置用于：根据权利要求1至10中任意一项所述的重建器械术野中心的控制方法对所述机械臂上的静平台进行位姿变换，对所述机械臂上的动平台进行旋转变换，或者根据权利要求11至41中的任意一项所述的机械臂的控制方法对所述机械臂上的静平台进行位姿变换。
  45. 一种手术机器人，其中，所述手术机器人包括：
    底座；以及
    根据权利要求44所述的机械臂组件。
  46. 一种可读存储介质，其上存储有可执行程序，其中，所述可执行程序被处理器执行时实现权利要求1至10中任意一项所述的重建器械术野中心的控制方法或者实现权利要求11至41中的任意一项所述的机械臂的控制方法。
  47. 一种计算机程序产品，其包括计算机指令，其中，所述计算机指令被处理器执行时实现权利要求1至10中任意一项所述的重建器械术野中心的控制方法或者实现权利要求11至41中的任意一项所述的机械臂的控制方法。
PCT/CN2022/098189 2021-06-11 2022-06-10 重建器械术野中心的控制方法、系统和存储介质 WO2022258054A1 (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
EP22819651.5A EP4353180A1 (en) 2021-06-11 2022-06-10 Control method and system for reconstructed surgical field center of instrument, and storage medium
US18/535,498 US20240123609A1 (en) 2021-06-11 2023-12-11 Control method of mechanical arm, mechanical arm assembly, and storage medium

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN202110682456.0 2021-06-11
CN202110682456.0A CN113545851B (zh) 2021-06-11 2021-06-11 重建器械术野中心的控制方法、系统、设备和存储介质

Related Child Applications (1)

Application Number Title Priority Date Filing Date
US18/535,498 Continuation US20240123609A1 (en) 2021-06-11 2023-12-11 Control method of mechanical arm, mechanical arm assembly, and storage medium

Publications (1)

Publication Number Publication Date
WO2022258054A1 true WO2022258054A1 (zh) 2022-12-15

Family

ID=78130763

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2022/098189 WO2022258054A1 (zh) 2021-06-11 2022-06-10 重建器械术野中心的控制方法、系统和存储介质

Country Status (4)

Country Link
US (1) US20240123609A1 (zh)
EP (1) EP4353180A1 (zh)
CN (2) CN113545851B (zh)
WO (1) WO2022258054A1 (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116277007A (zh) * 2023-03-28 2023-06-23 北京维卓致远医疗科技发展有限责任公司 位姿控制方法、装置、存储介质及控制器

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113545851B (zh) * 2021-06-11 2022-07-29 诺创智能医疗科技(杭州)有限公司 重建器械术野中心的控制方法、系统、设备和存储介质
CN114074329B (zh) * 2022-01-19 2022-04-15 成都博恩思医学机器人有限公司 一种机器人远心不动点的校验控制方法及相关装置
CN117503365A (zh) * 2022-07-28 2024-02-06 深圳市精锋医疗科技股份有限公司 手术机器人及其控制方法、控制装置
CN115816423A (zh) * 2022-09-20 2023-03-21 北京萌友智能科技有限公司 用于机器人的并联机构云台装置、位姿控制方法及控制器

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108015750A (zh) * 2018-01-15 2018-05-11 上海联影医疗科技有限公司 医用机械臂
CN108113755A (zh) * 2012-06-01 2018-06-05 直观外科手术操作公司 多端口手术机器人系统架构
CN108267979A (zh) * 2017-12-05 2018-07-10 北京航空航天大学 一种用于斯图尔特平台的控制设备及其控制方法
US20190142396A1 (en) * 2016-05-04 2019-05-16 The Johns Hopkins University Remote center of motion robot
CN110142762A (zh) * 2019-05-10 2019-08-20 武汉库柏特科技有限公司 一种机器人关节位置控制方法、装置和机器人
CN110815180A (zh) * 2019-10-31 2020-02-21 陕西科技大学 六自由度并联机器人运动分析建模及快速求解方法
CN111227943A (zh) * 2020-01-23 2020-06-05 诺创智能医疗科技(杭州)有限公司 手术机械臂的控制方法、计算机设备及一种手术机械臂
CN112894802A (zh) * 2020-12-28 2021-06-04 诺创智能医疗科技(杭州)有限公司 多级并联手术机械臂的控制方法及多级并联手术机械臂
CN113334390A (zh) * 2021-08-06 2021-09-03 成都博恩思医学机器人有限公司 一种机械臂的控制方法、系统、机器人及存储介质
CN113545851A (zh) * 2021-06-11 2021-10-26 诺创智能医疗科技(杭州)有限公司 重建器械术野中心的控制方法、系统、设备和存储介质

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8469945B2 (en) * 2006-01-25 2013-06-25 Intuitive Surgical Operations, Inc. Center robotic arm with five-bar spherical linkage for endoscopic camera
CN100520324C (zh) * 2007-02-06 2009-07-29 浙江大学 弹簧结构形式的多自由度并联机构广义力及力矩测试装置
CN104959975B (zh) * 2015-07-27 2017-01-18 吉林大学 运动可解耦的三维平动并联机构
CN105807712B (zh) * 2016-02-26 2018-07-24 南京航空航天大学 一种六自由度并联机器人正向运动学的对偶四元数求解的方法
CN106112980A (zh) * 2016-07-26 2016-11-16 清华大学 一种姿态调节装置
CN109669482A (zh) * 2018-11-15 2019-04-23 歌尔股份有限公司 云台控制方法、装置及设备
CN110464471B (zh) * 2019-09-10 2020-12-01 深圳市精锋医疗科技有限公司 手术机器人及其末端器械的控制方法、控制装置

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108113755A (zh) * 2012-06-01 2018-06-05 直观外科手术操作公司 多端口手术机器人系统架构
US20190142396A1 (en) * 2016-05-04 2019-05-16 The Johns Hopkins University Remote center of motion robot
CN108267979A (zh) * 2017-12-05 2018-07-10 北京航空航天大学 一种用于斯图尔特平台的控制设备及其控制方法
CN108015750A (zh) * 2018-01-15 2018-05-11 上海联影医疗科技有限公司 医用机械臂
CN110142762A (zh) * 2019-05-10 2019-08-20 武汉库柏特科技有限公司 一种机器人关节位置控制方法、装置和机器人
CN110815180A (zh) * 2019-10-31 2020-02-21 陕西科技大学 六自由度并联机器人运动分析建模及快速求解方法
CN111227943A (zh) * 2020-01-23 2020-06-05 诺创智能医疗科技(杭州)有限公司 手术机械臂的控制方法、计算机设备及一种手术机械臂
CN112894802A (zh) * 2020-12-28 2021-06-04 诺创智能医疗科技(杭州)有限公司 多级并联手术机械臂的控制方法及多级并联手术机械臂
CN113545851A (zh) * 2021-06-11 2021-10-26 诺创智能医疗科技(杭州)有限公司 重建器械术野中心的控制方法、系统、设备和存储介质
CN113334390A (zh) * 2021-08-06 2021-09-03 成都博恩思医学机器人有限公司 一种机械臂的控制方法、系统、机器人及存储介质

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116277007A (zh) * 2023-03-28 2023-06-23 北京维卓致远医疗科技发展有限责任公司 位姿控制方法、装置、存储介质及控制器
CN116277007B (zh) * 2023-03-28 2023-12-19 北京维卓致远医疗科技发展有限责任公司 位姿控制方法、装置、存储介质及控制器

Also Published As

Publication number Publication date
EP4353180A1 (en) 2024-04-17
CN115211970A (zh) 2022-10-21
CN113545851B (zh) 2022-07-29
US20240123609A1 (en) 2024-04-18
CN113545851A (zh) 2021-10-26

Similar Documents

Publication Publication Date Title
WO2022258054A1 (zh) 重建器械术野中心的控制方法、系统和存储介质
US20210001483A1 (en) Path-Modifying Control System Managing Robot Singularities
CN107589934B (zh) 一种关节型机械臂逆运动学解析解的求取方法
US6047610A (en) Hybrid serial/parallel manipulator
Yang et al. Feasibility study of a platform type of robotic manipulators from a kinematic viewpoint
JP3207728B2 (ja) 冗長マニピュレータの制御方法
US5590034A (en) Method for controlling the movement of an industrial robot at and near singularities
CN111300425B (zh) 一种超冗余机械臂末端轨迹运动规划方法
CN109176488B (zh) 一种柔性机器人运动学标定方法及系统
US9321175B2 (en) Robotic manipulator articulation techniques
JP2694669B2 (ja) ロボットの動作制御方法
CN111890349A (zh) 一种四自由度机械臂运动规划方法
KR20210134955A (ko) 운동학적 잉여 작동을 갖는 병렬 메커니즘
CN109129469B (zh) 机械臂运动学逆解方法、装置及机械臂
CN113334381A (zh) 一种可运动解耦的连续体机器人控制方法
JP2010110878A (ja) 多関節型ロボット装置およびその制御方法
JPH01267706A (ja) ロボットの制御方法
CN111113425A (zh) 一种有寄生运动的五自由度串并联机器人运动学逆解求解方法
CN113722864B (zh) 一种7自由度冗余机械臂逆运动学求解方法及系统
JP4230196B2 (ja) 位置決め演算方法および位置決め演算装置
JPS62251284A (ja) 多脚機構の協調制御方式
JP3331674B2 (ja) ７軸多関節ロボットマニピュレータの肘回転方法
Peng et al. Kinematics and orientation capability of a family of 3-DOF parallel mechanisms
Jiang et al. Attitude Interpolation Algorithm for Industrial Robot Based on Quaternion Method
JP2676721B2 (ja) 多関節型ロボットの制御装置

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 22819651

Country of ref document: EP

Kind code of ref document: A1

WWE Wipo information: entry into national phase

Ref document number: 2022819651

Country of ref document: EP

NENP Non-entry into the national phase

Ref country code: DE

ENP Entry into the national phase

Ref document number: 2022819651

Country of ref document: EP

Effective date: 20240111