Pose data correction method and electronic device

Abstract

The application provides a pose data correction method and an electronic device. By introducing a dynamic double constraint mechanism, the application significantly reduces the risk of injury to the patient caused by the end tool of the surgical robot during operation. Specifically, when it is detected that the preset trigger condition is met, the tool motion trajectory is corrected in real time based on the spatial vector relationship between the initial target pose data and the preset point, so that the rotation angle constraint and the operation depth constraint are simultaneously satisfied; the first constraint condition limits the maximum deviation angle of the tool axis and the preset vector to ensure that the rotation range of the end tool around the telecentric fixed point (the preset point) is controlled, thereby avoiding tearing of the tissue at the incision or lateral deviation of the instrument due to excessive rotation; the second constraint condition sets the maximum allowed distance from the end of the tool to the telecentric fixed point to accurately limit the invasion depth of the end tool, thereby preventing damage to internal organs or blood vessels due to excessive operation.

Description

Technical Field

[0001] This invention relates to the technical field of surgical robot control, specifically to a method for correcting pose data and an electronic device. Background Technology

[0002] A surgical robot is a medical device that integrates a robotic arm, 3D imaging, and a control system. Surgical robots can perform more precise and minimally invasive procedures under the control of a surgeon.

[0003] Existing methods for controlling the motion of end-effectors in surgical robots present technical problems that could potentially harm patients. Summary of the Invention

[0004] The purpose of this invention is to overcome the above-mentioned technical deficiencies and provide a method for correcting pose data and an electronic device to solve the technical problem that the motion control method of the end effector of the surgical robot in the related art is prone to causing harm to the patient.

[0005] To achieve the above-mentioned technical objectives, the present invention adopts the following technical solution: In a first aspect, the present invention provides a method for correcting pose data, the method comprising: When a preset trigger condition is detected, a first pose data is generated based on a preset motion control algorithm; wherein, the first pose data represents the initial target pose data of the robotic arm end effector. Based on the spatial vector relationship between the initial target pose data and the preset point, the first pose data is corrected to obtain the second pose data; wherein, the second pose data is pose data that satisfies the first constraint condition and the second constraint condition; wherein, the first constraint condition is a constraint condition that enables the end-effector to avoid excessive rotation, thereby preventing the end-effector from damaging the protected object; the second constraint condition is a constraint condition that enables the end-effector to avoid excessive penetration, thereby preventing the end-effector from damaging the protected object.

[0006] In a second aspect, the present invention provides an electronic device, comprising: a memory, and one or more processors communicatively connected to the memory; the memory stores instructions executable by the one or more processors, the instructions being executed by the one or more processors to cause the one or more processors to implement the method described above.

[0007] Beneficial effects: This invention significantly reduces the risk of injury to patients caused by surgical robot end-effectors during operation by introducing a dynamic dual constraint mechanism. Specifically, when a preset trigger condition is detected, the tool's trajectory is corrected in real time based on the spatial vector relationship between the initial target pose data and the preset point, ensuring that it simultaneously satisfies rotation angle constraints and operation depth constraints. The first constraint condition limits the maximum deviation angle between the tool axis and the preset vector, ensuring that the rotation range of the end-effector around the distal fixed point (preset point) is controlled, avoiding tissue tearing at the incision site or lateral displacement of the instrument due to excessive rotation. The second constraint condition precisely limits the depth of penetration of the end-effector by setting the maximum allowable distance from the tool tip to the distal fixed point, preventing damage to internal organs or blood vessels due to excessively deep operation. Attached Figure Description

[0008] Figure 1 This is a flowchart illustrating a pose data correction method provided in an embodiment of the present invention; Figure 2 This is a schematic diagram of the constraint space provided in an embodiment of the present invention; Figure 3 This is a schematic diagram illustrating the correction of the end-effector pose data at the next moment when the first data is within a first numerical range, as provided in an embodiment of the present invention; wherein, Figure 3 The left figure in the diagram is a schematic diagram showing that the position of the end effector at the next moment is located inside the constraint space; Figure 3 The right figure in the diagram is a schematic diagram showing that the position of the end effector in the next moment exceeds the constraint space; Figure 4 This is a schematic diagram illustrating the correction of the end-effector pose data at the next moment when the first data is within the second numerical range, as provided in this embodiment of the invention; wherein, Figure 4 The left image shows the position of the end effector at the next moment, after projection, located on the boundary surface; Figure 4 The right figure shows the situation where the position of the end tool at the next moment exceeds the boundary surface after projection; Figure 5 This is a schematic diagram illustrating the correction of the end-effector pose data at the next moment when the first data provided in this embodiment of the invention is within the third and fourth numerical ranges; wherein, Figure 5 The left-middle graph shows the case where the first data point is in the fourth value range; Figure 5 The middle right figure shows the case where the first data point is in the third value range; Figure 6 This is a block diagram of an electronic device used in an embodiment of the present invention. Detailed Implementation

[0009] To enable those skilled in the art to better understand the present application, the technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present application, and not all embodiments. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative effort should fall within the scope of protection of the present application.

[0010] In related technologies, with the rapid development of surgical robot technology, minimally invasive surgery has gradually become one of the common and important surgical methods in modern medicine. Compared with traditional surgery, minimally invasive surgery makes the surgical process more precise and less traumatic through small incisions, thereby accelerating patient recovery. However, minimally invasive surgery has extremely high requirements for operational precision and safety, especially in the process of surgery assisted by surgical robots, where how to precisely control the movement of the end effector of the robotic arm is crucial.

[0011] In existing surgical robotic technologies, the end effector of a robotic arm typically uses pre-set control algorithms for motion planning and path control. These control algorithms rely on the real-time pose data of the robotic arm and ensure that the tool can perform operations within a predetermined surgical area through precise trajectory adjustments. However, existing motion control methods still have a significant problem: during the movement of the end effector, it is prone to damaging the patient's tissues due to excessive rotation angles or depths.

[0012] Specifically, in robot-assisted minimally invasive surgery, excessive rotation or insertion of the end effector can lead to unnecessary collisions or damage between the tool and the patient's tissues. This phenomenon stems from the insufficient constraints on the movement of the end effector in existing control methods. Particularly in complex surgical environments, the tool lacks precision when contacting tissue or bone, easily leading to misoperation or excessive movement, thus posing potential harm to the patient. For example, some control methods lack real-time monitoring of the end effector's rotation angle, causing the tool to deviate from its intended trajectory during surgery, resulting in unnecessary injury. Simultaneously, the lack of real-time adjustment of the distance between the tool and the protected object can cause the tool to insert too deeply or deviate from its target, increasing surgical risks. Therefore, these technological deficiencies severely impact the precision and safety of surgical robots during surgery.

[0013] In related technologies, there are technical solutions that achieve rigid constraints through a physical telecentric fixed point (RCM). Specifically, a mechanical structure (e.g., a parallelogram linkage) can force the tool to move around a fixed point to ensure that the force at the incision site is minimized. Understandably, on the one hand, the fixed point of the machine is fixed in position and cannot be dynamically adjusted according to surgical needs. For example, in thoracoscopic surgery, changes in patient position may cause the incision position to shift, and existing mechanical structures cannot adaptively correct this, requiring manual repositioning, prolonging surgical time and increasing operational complexity. On the other hand, when the tool axis is not strictly aligned with the fixed point of the machine, rotation around the point will generate lateral shear force. If there is an angle between the axis of the end-effector and the normal vector of the fixed point of the machine, the tissue at the incision site will bear asymmetrical loads, and long-term operation may lead to tissue tearing or postoperative complications.

[0014] In related technologies, there are technical solutions that implement constraints through software. Specifically, virtual walls can be set up to constrain the range of motion of the end effector. It is understandable that these solutions often constrain the movement of the end effector in a single dimension, such as limiting the axial depth or lateral offset, lacking dynamic constraints on rotational angles.

[0015] Therefore, in summary, the existing motion control methods for the end effector of surgical robots have technical problems that can easily cause harm to patients.

[0016] like Figure 1 As shown, this embodiment provides a method for correcting pose data. The execution subject of the method can be the control device of a surgical robot, which can be an FPGA, DSP, or a dedicated ASIC chip, or it can be a server. The server can be a high-performance computing server, a GPU server, an edge computing server, or a cloud server, etc.

[0017] The method may include: Step S10: When a preset trigger condition is detected, a first pose data is generated based on a preset motion control algorithm; wherein, the first pose data represents the initial target pose data of the robotic arm end tool. Specifically, the triggering conditions may include: The end-effector enters the preset area of ​​the preset point; The force data experienced by the end effector exceeds the preset force data threshold; Received a user-input command to switch motion modes; The end-effector's movement time exceeds a preset time threshold; or, The end effector's movement speed exceeds a preset speed threshold; In this embodiment, the method of the embodiment can be triggered when any of the triggering conditions described above is met. For example, the method of the embodiment can be triggered once the end effector is detected to enter the preset area of ​​a preset point.

[0018] In this embodiment, the method of the embodiment can be triggered when multiple triggering conditions described above are detected.

[0019] In this embodiment, the method of the embodiment can be triggered only when all of the above-described triggering conditions are detected.

[0020] In this embodiment, the preset motion control algorithm can be a motion control algorithm based on impedance control.

[0021] In this embodiment, the preset motion control algorithm can be a motion control algorithm based on inverse kinematics control.

[0022] In this embodiment, the preset motion control algorithm can be a motion control algorithm based on model predictive control (MPC).

[0023] In this embodiment, the preset motion control algorithm can be a motion control algorithm based on fuzzy control.

[0024] In this embodiment, the preset motion control algorithm can also be a motion control algorithm based on force / position hybrid control.

[0025] In this embodiment, the preset motion control algorithm can also be a motion control algorithm based on admittance control. It is understood that motion control algorithms based on admittance control enable the robot to have better compliance, allowing the robotic arm to adapt to changes in the external environment. When the end effector comes into contact with the patient's tissue, admittance control can adjust the movement according to changes in the contact force, avoiding excessive force or excessive depth of entry, thereby effectively preventing harm to the patient. For example, in minimally invasive surgery, admittance control allows the robotic arm to automatically adjust its force according to different tissue hardness, resistance, and other environmental factors, thereby ensuring the accuracy and safety of the operation.

[0026] In this embodiment, the step of generating the first pose data based on a preset motion control algorithm may include: Step S12: Detect the pose data of the current end effector; Step S14: Input the current pose data of the end effector into a preset motion control algorithm to generate the first pose data; wherein, the first pose data represents the pose data of the end effector relative to the next moment of the current moment.

[0027] In this embodiment, the detection of the current end-effector's pose data can be achieved through a pre-set detection device. Specifically, the detection device can be a built-in detection device within the surgical robot system, capable of measuring the end-effector's pose in real time. More specifically, the detection device can be an optical measurement device, which acquires the end-effector's pose data by capturing the relative position between the end-effector and a reference marker. For example, it could be a stereo vision system, which acquires the end-effector's position and orientation in three-dimensional space using multiple cameras. By comparing the image of the tool with a pre-set template or marker point, the precise position and angle of the end-effector are calculated. More specifically, the detection device can be a laser measurement device, which calculates the precise position of the end-effector by emitting a laser beam and measuring the time it takes for the beam to return. The detection device can also be an electromagnetic measurement device, which uses changes in the electromagnetic field by deploying electromagnetic sensors and receivers in space to detect the precise position and orientation of the end-effector.

[0028] In this embodiment, the first pose data may be pose data representing an initial target point. It is understood that the initial target point is a spatial location that the end-effector should move to in the next moment, calculated by a preset motion control algorithm based on the pose data of the end-effector at the current moment.

[0029] Understandably, the pose data of this location point needs to be corrected in many cases to satisfy the first and second constraints. For example, the pose and position of this location point can be corrected to satisfy the first and second constraints. Understandably, if only the position of this location point (initial target point) is adjusted without correcting the pose, rotation over-limit problems may occur. For example, although the tool tip does not exceed the depth of the base region (e.g., it could be a preset virtual wall), its pose deviates from the preset axis, and the rotation angle around the preset point (RCM point) exceeds the maximum deviation angle, causing the incisional tissue to be subjected to asymmetric shear force (violating the first constraint). Moreover, pose errors may cause the side of the end tool or non-working areas to come into contact with sensitive tissues (e.g., blood vessels, nerves).

[0030] It is also understandable that if only the attitude is corrected without correcting the position, the end effector may exceed the safe depth limit. For example, the end effector may maintain the correct orientation but penetrate excessively along the axis, exceeding the depth allowed by the base region (violating the second constraint).

[0031] Step S12: Based on the spatial vector relationship between the initial target pose data and the preset point, correct the first pose data to obtain the second pose data; wherein, the second pose data is pose data that satisfies the first constraint condition and the second constraint condition; wherein, the first constraint condition is a constraint condition that enables the end-effector to avoid excessive rotation, thereby preventing the end-effector from damaging the protected object; the second constraint condition is a constraint condition that enables the end-effector to avoid excessive penetration, thereby preventing the end-effector from damaging the protected object.

[0032] In this embodiment, the preset point can be a user-defined distal fixed point (RCM), which can be located above the patient's affected skin or at the incision site of the patient's affected area.

[0033] Specifically, in a possible scenario, before surgery begins, the surgeon uses the control interface of the surgical robot system to set the telecentric fixed point. For example, the surgeon can enter the telecentric fixed point setting mode via the surgical robot's console or touchscreen. The control interface can display a 3D model of the patient or preoperative images, and the surgeon can mark a reference point on the image; this point will serve as the location of the telecentric fixed point. The telecentric fixed point can be set on the skin surface of the affected area, especially above the incision site in minimally invasive surgery. It can also be set at the incision location, particularly in situations requiring precise control of the surgical tool's path. Setting the telecentric fixed point at the incision location ensures the flexibility of the end effector's movement while preventing excessive penetration during surgery, thus ensuring the safety of the procedure.

[0034] In this embodiment, the protected object refers to a part, target, component, device, or structure that is specifically protected during the movement of the end-effector in different application scenarios.

[0035] In this embodiment, the protected object may be a patient's blood vessels, nerves, tissues, or organs during minimally invasive surgery.

[0036] In this embodiment, in the field of precision instrument repair, the protected object can be a precision component or system assembly inside the instrument. For example, it can be an electronic component (circuit board, chip), a mechanical structure (gear, bearing, transmission system), or an optical component, etc.

[0037] In this embodiment, in the aerospace field, the protected object can be an engine assembly or the electronic system of a spacecraft.

[0038] In this embodiment, in the field of archaeology, the protected object can be ceramics and porcelain, metal artifacts, wooden artifacts, or paper artifacts.

[0039] In this embodiment, the spatial vector relationship may include: The position vector relationship can be the displacement vector between the position point of the end tool tip and the preset point. This displacement vector can be expressed as the spatial offset of the end tool tip relative to the RCM point.

[0040] The axial vector of the end-effector can be the direction of the end-effector's axis or a unit vector pointing from the proximal end (near the robotic arm) to the distal end (operating end). This axial vector can be used to describe the orientation of the end-effector and to determine whether the rotation angle of the end-effector around the RCM point exceeds the limit.

[0041] A preset vector, which can be a preset reference direction vector. For example, it can be a plane normal vector of a preset base region (e.g., the base region can be a virtual plane or a virtual wall used to constrain the movement of the end effector; this normal vector is the normal vector pointing from the base region to the patient). It can be understood that this preset vector serves as the reference direction for the rotation angle, used to constrain the maximum deviation angle between the tool axis and the preset direction.

[0042] In this embodiment, the first constraint is a constraint that prevents the end-effector from rotating excessively, thereby preventing the end-effector from damaging the protected object.

[0043] Specifically, the first constraint condition can be a constraint condition that limits the maximum deviation angle between the axial vector of the end-effector and a preset vector. The axial vector of the end-effector is a vector representing the distance from the end of the end-effector closest to the robotic arm to the end furthest from the robotic arm. The preset vector can be a vector pointing from a preset point (RCM point) to a virtual wall (base region, which can be a virtual plane or a virtual curved surface), that is, the normal vector of the virtual wall pointing to the patient.

[0044] The first constraint can also be a constraint on the maximum angular velocity of the end-effector rotating around a preset point. Alternatively, it can be a constraint on the maximum angular acceleration of the end-effector rotating around a preset point. This constraint can prevent inertial runaway or tool vibration caused by rapid rotation, thus avoiding instantaneous impact on the incisional tissue.

[0045] The first constraint can also be a constraint on the dynamic maximum deviation angle between the axial vector of the end-effector tool and a preset vector. For example, the user can dynamically adjust the maximum deviation angle based on real-time intraoperative data.

[0046] The first constraint can also be a constraint on the instantaneous radius of curvature of the tip trajectory of the end-effector.

[0047] In this embodiment, the second constraint is a constraint that prevents the end-effector from penetrating too deeply, thereby preventing the end-effector from damaging the protected object.

[0048] Specifically, the second constraint condition may be a constraint condition that limits the maximum distance from the end of the end tool to a preset point.

[0049] The second constraint can be a constraint that limits the maximum amount of movement of the end tool along the tool axis.

[0050] The second constraint can be a constraint on the contact force of the end-effector. It is understood that the contact force between the end-effector and the protected object should also be limited to avoid the end-effector applying excessive force to the protected object, thereby causing injury. Excessive contact force may indicate that the end-effector is penetrating deep into the target area or contacting sensitive tissue.

[0051] In this embodiment, the first pose data and the second pose data can be pose matrices.

[0052] In this embodiment, the first pose data and the second pose data can be in the form of position vectors and Euler angles or quaternions.

[0053] In one specific implementation, the initial target pose data indicates an initial target point existing in space; the first constraint condition includes a constraint condition constraining the maximum deviation angle between the axial vector of the end effector and a preset vector, and the second constraint condition includes a constraint condition constraining the maximum distance from the end of the end effector to the preset point; wherein, the axial vector of the end effector is a vector representing the distance from the end of the end effector near the robotic arm to the end far from the robotic arm; the step of correcting the first pose data based on the spatial vector relationship between the initial target pose data and the preset point to obtain the second pose data may include: First, based on the spatial vector relationship between the initial target point and the preset point, first data and second data are calculated; wherein, the first data represents the angle between the vector pointing from the preset point to the initial target point and the preset vector; and the second data represents the distance data from the initial target point to the preset point. Then, it is determined whether the first data and the second data satisfy the first constraint condition corresponding to the first data and the second constraint condition corresponding to the second data, respectively. Finally, if at least one constraint is not satisfied, the pose data of the initial target point is corrected based on the first constraint and / or the second constraint to obtain the second pose data.

[0054] Understandably, based on the second pose data obtained after the implementation modification, the control device of the surgical robot can generate corresponding motion control signals. These motion control signals are used to control the movement of the robotic arm, thereby causing the end effector to move to the position and direction indicated by the second pose data. For example, the control device can calculate the joint angles required to reach the second pose data based on the inverse kinematics algorithm of the robotic arm, then generate motor control signals based on these joint angles, and finally send the motor control signals to the motor driver to drive the motor.

[0055] The beneficial effect of this embodiment is that by introducing a dynamic dual constraint mechanism, the risk of injury to the patient caused by the surgical robot's end effector during operation is significantly reduced. Specifically, when a preset trigger condition is detected, the tool's motion trajectory is corrected in real time based on the spatial vector relationship between the initial target pose data and the preset point, so that it simultaneously satisfies the rotation angle constraint and the operation depth constraint. The first constraint condition ensures that the rotation range of the end effector around the distal fixed point (preset point) is controlled by limiting the maximum deviation angle between the tool axis and the preset vector, avoiding tissue tearing at the incision site or lateral displacement of the instrument due to excessive rotation. The second constraint condition precisely limits the invasive depth of the end effector by setting the maximum allowable distance from the tool tip to the distal fixed point, preventing damage to internal organs or blood vessels due to excessive operation.

[0056] In some implementations, the preset triggering conditions include: The end tool enters the preset area of ​​the preset point.

[0057] In this embodiment, the preset region is a three-dimensional spherical or cylindrical space centered at a preset point (RCM point) with a preset length as its radius. When the distance between the tip of the end effector and the preset point is less than the radius, it is determined that the end effector has entered the preset region of the preset point. It is understood that this method can activate constraints in advance when the end effector approaches the cut, avoiding accidental collisions caused by rapid approach.

[0058] The force data experienced by the end tool exceeds the preset force data threshold.

[0059] In this embodiment, the contact force / torque (force data) between the end tool and the tissue is measured by a six-dimensional force sensor, and a correction is triggered when the magnitude of the contact force / torque exceeds a threshold (force data threshold).

[0060] The force data can be force data, torque data, or the magnitude of force data and torque data.

[0061] The user input command to switch motion modes was received.

[0062] In this embodiment, doctors can actively switch operating modes (e.g., "free mode" → "restraint mode") via foot pedal, button, or voice command.

[0063] The movement time of the end effector exceeds the preset time threshold.

[0064] In this embodiment, a correction is triggered when the end effector moves continuously in a certain area for a period of time exceeding a preset time threshold. For example, the dwell time of the end effector in the area can be recorded by the clock built into the surgical robot.

[0065] Alternatively, the end effector's movement speed exceeds a preset speed threshold.

[0066] In this embodiment, the motion speed can be either linear velocity or angular velocity. Therefore, the velocity threshold can be either a linear velocity threshold or an angular velocity threshold.

[0067] This embodiment achieves dynamic control and real-time adjustment of the end effector's movement by setting a series of preset trigger conditions. These trigger conditions automatically trigger a pose data correction process when the end effector enters a preset area, experiences a force exceeding a threshold, receives a user's mode switching command, or exceeds time or speed limits during movement, ensuring the end effector remains within a predetermined safe range. This triggering mechanism responds promptly to external changes, preventing the end effector's movement from deviating from its expected trajectory due to irregular operations or external interference during surgery, reducing the risk of misoperation, and effectively improving operational accuracy and safety. Dynamic adjustment enables precise control of the surgical tool in complex, real-time changing surgical environments, enhancing the reliability and adaptability of the surgical robot system.

[0068] In some embodiments, the initial target pose data indicates an initial target point existing in space; the first constraint condition includes a constraint condition constraining the maximum deviation angle between the axial vector of the end-effector and a preset vector, and the second constraint condition includes a constraint condition constraining the maximum distance from the end of the end-effector to the preset point; wherein, the axial vector of the end-effector is a vector representing the distance from the end of the end-effector closer to the robotic arm to the end farther from the robotic arm; The step of correcting the first pose data based on the spatial vector relationship between the initial target pose data and the preset point to obtain the second pose data includes: Based on the spatial vector relationship between the initial target point and the preset point, first data and second data are calculated; wherein, the first data represents the angle between the vector pointing from the preset point to the initial target point and the preset vector; and the second data represents the distance data from the initial target point to the preset point.

[0069] In this embodiment, the preset vector can be the normal vector of a preset base region (e.g., a virtual plane). It is understood that this virtual plane lies between the end-effector and the patient. This virtual plane can be circular, rectangular, triangular, or other shapes.

[0070] Specifically, if the base region is a plane, then the preset vector can be the normal vector of that plane. If the base region is a curved surface or an irregular surface, then the preset vector can be the local normal vector of a feature point of that curved or irregular surface. Similarly, this local normal vector is a normal vector pointing towards the patient.

[0071] More specifically, the feature point can be the lowest point of the base region.

[0072] This feature point can be the center of curvature of the base region.

[0073] This feature point can be a tangent point in the basal region. This feature point can also be the centroid of the base region.

[0074] This feature point can also be the surface center point of the base region.

[0075] Determine whether the first data and the second data satisfy the first constraint condition corresponding to the first data and the second constraint condition corresponding to the second data, respectively.

[0076] In this embodiment, the first data (angle data) can be directly compared with the first constraint (maximum deviation angle, for example, 45 degrees, 50 degrees, or 60 degrees, etc.). If the first data is less than or equal to the first constraint, it means that the first data satisfies the first constraint. If the first data is greater than the first constraint, it means that the first data does not satisfy the first constraint.

[0077] Accordingly, the second data (distance data) can be directly compared with the second constraint (maximum distance). If the second data is less than or equal to the second constraint, it means that the second data satisfies the second constraint. If the second data is greater than the second constraint, it means that the second data does not satisfy the second constraint.

[0078] If at least one constraint is not satisfied, the pose data of the initial target point is modified based on the first constraint and / or the second constraint to obtain the second pose data.

[0079] In this embodiment, if one constraint condition is not met, the pose data of the initial target point can be corrected based on the first constraint condition and / or the second constraint condition to obtain the pose data of the final target point (second pose data).

[0080] Specifically, if the first constraint condition is not met, the pose data of the initial target point can be corrected based on the first constraint condition to obtain the pose data of the final target point (second pose data). For example, the rotation of the end effector at the next moment can be restricted to the maximum deviation angle (for example, the vector pointing from the preset point to the initial target point can be directly projected onto the boundary surface of the constraint space; it can be understood that the constraint space can be the constraint space formed by the preset point and the base region. For example, in the case of a conical constraint space, the initial target point can be directly projected onto the surface of the cone. Therefore, the pose matrix can be regenerated based on the corrected axial vector). Alternatively, an angle value can be randomly determined within the range of zero degrees to the maximum deviation angle and assigned to the final target point.

[0081] If the second constraint is not met, the pose data of the initial target point can be corrected based on the second constraint to obtain the pose data of the final target point (second pose data). For example, the penetration distance of the end effector at the next moment can be limited to the maximum distance (that is, the initial target point is withdrawn along the axis to the surface of the base region, that is, the bottom surface of the constraint space), or the penetration distance of the end effector at the next moment can be limited to the maximum distance multiplied by a preset reduction factor, which can be a factor less than 1 (that is, the initial target point is withdrawn along the axis a greater distance, so that it is withdrawn to the interior of the constraint space).

[0082] In this embodiment, if neither constraint is satisfied, the pose data of the final target point can be obtained by performing corrections based on the first and second constraints and the pose data of the initial target point.

[0083] Specifically, the pose data of the initial target point can be corrected based on the first constraint condition. Then, it is determined whether the pose data after the first correction meets the second constraint condition. If it does not meet the second constraint condition, the pose data after the first correction is corrected again based on the second constraint condition to obtain the pose data of the final target point.

[0084] This implementation significantly enhances the adaptability of the surgical robot system to complex surgical areas through dynamic correction based on multiple constraints. During the surgery, the position and orientation of the end effector are strictly controlled to ensure it does not deviate from the predetermined trajectory or penetrate excessively. This method combines dual constraints of rotation angle and depth, enabling real-time adjustment of the tool's motion path according to the actual situation, effectively addressing different surgical needs on planar, curved, or irregular surfaces. Furthermore, this method prioritizes satisfying the rotation angle constraint to ensure the end effector's rotation does not exceed a safe range, and then further precisely controls it through depth constraints, ensuring the tool's safety and stability. This adaptive correction process allows the system to flexibly respond to different surgical environments, improving operational accuracy and safety.

[0085] In some implementations, the initial target pose data indicates an initial target point existing in space; the first constraint includes a constraint on the maximum deviation angle between the axial vector of the end-effector and a preset vector, and the second constraint includes a constraint on the maximum dynamic distance from the end of the end-effector to the preset point when the end-effector is rotated to different rotation angles; wherein, the axial vector of the end-effector is a vector representing the distance from the end of the end-effector closer to the robotic arm to the end farther from the robotic arm.

[0086] Specifically, in the implementation scheme, the second constraint condition ensures that the end-effector always remains inside the constraint space or on the bottom surface (base area) by dynamically adjusting the maximum allowable distance from the end-effector to the preset point (RCM point).

[0087] It is understandable that, for example, when the constraint space is a conical constraint space, the constraints on the end effector will differ depending on its target position (i.e., the pose of the end effector at the next moment). For instance, if the end effector moves from a preset point to the side of the cone and passes through that side, then in this case, the dynamic maximum distance is actually the distance between the preset point (RCM point, which is also the vertex of the cone) and the target position, passing through the intersection of the vector from that point and the side (or the side surface), and the preset point. The significance of this setting is that regardless of the target point of the end effector at the next moment (i.e., when the end effector is rotated to different rotation angles), the end effector should be constrained accordingly to prevent it from exceeding the constraint space. Similarly, it is understandable that when the rotation angle is 0, meaning the end effector's position at the next moment is directly below the constraint point, the constraint is minimal, and the dynamic maximum distance is maximum. Correspondingly, when the rotation angle is the maximum deviation angle in the first constraint condition, the constraint is maximum, and the dynamic maximum distance is minimum.

[0088] Understandably, by using the second constraint in this implementation scheme to dynamically calculate the intersection point between the target position direction and the side of the constraint space (e.g., a cone), the second constraint achieves omnidirectional restriction on the end effector. As the end effector moves in different directions, the maximum permissible distance is dynamically adjusted according to the rotation angle: a maximum depth is allowed along the axial direction, while the range gradually shrinks as it approaches the side of the constraint space. This mechanism not only solves the limitations of traditional fixed-depth constraints but also provides mathematical guarantees for the precise protection of high-risk areas, significantly improving the safety and reliability of surgical robot motion control.

[0089] The step of correcting the first pose data based on the spatial vector relationship between the initial target pose data and the preset point to obtain the second pose data includes: Step S122: Calculate first data based on the spatial vector relationship between the initial target point and the preset point; wherein, the first data is the angle between the vector pointing from the preset point to the initial target point and the preset vector.

[0090] Step S124: Determine the numerical range in which the first data is located; wherein the numerical range is a numerical range divided based on the first constraint condition.

[0091] Step S126: Based on the numerical range in which the first data is located, the first pose data is corrected to obtain intermediate pose data.

[0092] Step S128: Determine whether the intermediate pose data satisfies the second constraint condition.

[0093] Step S1210: If the condition is not met, the intermediate pose data is corrected using the second constraint to obtain the second pose data.

[0094] This implementation scheme significantly enhances the safety and flexibility of surgical robot end-effector movement by introducing a dynamically adjustable second constraint. Particularly in complex surgical environments, the dynamic maximum distance constraint of the end-effector at different rotation angles can be adjusted in real time according to the actual position and rotation angle of the tool, ensuring that the tool always remains within a safe constraint space. For example, when the end-effector approaches the side of the constraint space, the maximum permissible distance gradually decreases, preventing the tool from exceeding the predetermined safe range, providing more precise protection, especially in high-risk areas. By calculating the intersection of the target position and the side of the constraint space, this dynamic correction mechanism overcomes the limitations of traditional fixed-depth constraints, achieving omnidirectional restriction of the end-effector, thereby ensuring the accuracy and safety of tool operation. This scheme not only improves the adaptability of the surgical robot system, enabling it to flexibly respond to different surgical needs, but also provides strong mathematical support for high-risk operations in complex surgical areas, significantly improving the reliability and safety of surgical robot motion control.

[0095] In some embodiments, the numerical range includes a first numerical range, a second numerical range, a third numerical range, and a fourth numerical range; wherein, the first numerical range represents the range from zero degrees to the maximum deviation angle, the second numerical range represents the range from the maximum deviation angle to ninety degrees, the third numerical range represents the range from ninety degrees to one hundred and eighty degrees minus the maximum deviation angle, and the fourth numerical range represents the range from one hundred and eighty degrees minus the maximum deviation angle to one hundred and eighty degrees. The step of correcting the first pose data based on the numerical range in which the first data falls to obtain intermediate pose data includes: When the first data is within the first numerical range, the first pose data is used as the intermediate pose data.

[0096] When the first data is within the second numerical range, the vector pointing from the preset point to the initial target point is projected onto the boundary surface of the constraint space to obtain the pose data of the projected position point; wherein, the constraint space is a constraint space with the preset point as the vertex, a base region at a preset distance from the preset point as the bottom surface, and the boundary surface connecting the vertex and the bottom surface; wherein, the base region is set between the preset point and the protected object.

[0097] In this embodiment, the base region is a virtual plane, a virtual curved surface, or a virtual irregular surface, etc. The base region can be circular, elliptical, rectangular, triangular, or various irregular shapes, etc.

[0098] The preset point and the base area can form a constraint space, which can be used to constrain the movement of the end effector.

[0099] In one possible implementation, the constraint space can be constructed by radiating rays from a preset point to the boundary of the base region, thereby forming a complete closed constraint space. The connecting line between the preset point and the boundary of the base region constitutes the aforementioned boundary surface.

[0100] In one possible implementation, the constraint space can be constructed as follows: a spherical constraint space is formed with a preset point as the center and the constraint distance as the radius. In this case, the position of the end effector is restricted within this sphere, preventing the tool from penetrating beyond the predetermined safe depth.

[0101] In one possible implementation, the constraint space can be constructed by generating the elliptical constraint space centered at a preset point using a predefined elliptical equation.

[0102] In one possible implementation, the constraint space can be constructed by directly modeling the basal region using the patient's CT or other medical images.

[0103] When the first data is in the third numerical range, firstly, the vector projection from the preset point to the initial target point is inverted, and then the inverted vector is projected onto the boundary surface of the constraint space to obtain the pose data of the projected position point.

[0104] When the first data is in the fourth numerical range, firstly, the vector projection pointing from the preset point to the initial target point is inverted, and then the pose data of the position point indicated by the inverted vector is used as the intermediate pose data.

[0105] This implementation scheme effectively enhances the precision control and adaptability of the end effector by introducing multiple numerical interval divisions and a projection correction mechanism. By dividing the rotation angle into four numerical intervals, each interval employs a different correction strategy, enabling flexible responses to the end effector's motion requirements at different rotation angles. In the first numerical interval, the initial pose data is directly used as intermediate pose data, ensuring rapid response in simple cases. In the second, third, and fourth numerical intervals, by projecting a vector from a preset point to the initial target point onto the boundary surface of the constraint space or inverting the projection, the position of the end effector on complex paths can be precisely constrained, preventing the tool from exceeding the predetermined safety range. This mechanism not only ensures the dynamic adjustment of the end effector at different angles but also provides protection for high-risk surgical areas. Through this method, the system can achieve efficient and safe operation while maintaining precision, significantly improving the flexibility, reliability, and safety of surgical robot motion control.

[0106] In some embodiments, the base region is a plane, a curved surface, an irregular surface with at least a portion of a plane, or an irregular surface with at least a portion of a curved surface.

[0107] Specifically, when the base region is a plane, the plane equation can be defined by preset points and normal vectors to obtain the base region.

[0108] When the base region is a curved surface, a spherical equation or an ellipsoidal equation can be defined by defining a preset point as the center and a preset distance as the radius.

[0109] When the base region is an irregular surface with at least some planar surfaces or at least some curved surfaces, a hybrid modeling approach can be used, using planar surfaces in key areas (e.g., near incisions) and curved surfaces (e.g., organ surfaces) in other areas.

[0110] In this embodiment, the base region can be defined as a plane, curved surface, partially plane, or irregular surface. Its diverse geometric forms significantly enhance the adaptability and safety of the constraint space. Planar bases (e.g., virtual walls) can quickly achieve axial depth constraints, simplifying calculations and improving real-time performance. Curved bases (e.g., organ surface reconstruction models) can conform to complex anatomical structures, avoiding rigid, one-size-fits-all constraints. This multimodal base design not only expands the application scenarios of the constraint space but also reduces the error accumulation of correction algorithms through geometric fit, ultimately achieving high-precision control of the end-effector's trajectory and ensuring comprehensive protection of the patient's skin, blood vessels, and critical tissues.

[0111] In some embodiments, when the base region is a circular plane and the constraint space is conical, the step of projecting the vector pointing from the preset point to the initial target point onto the boundary surface of the constraint space to obtain the pose data of the projected position point includes: First, calculate the unit vector projected from the preset point to the initial target point onto the base region.

[0112] Specifically, this means projecting the vector onto the bottom surface (base region) of the constrained space.

[0113] It is understandable that in this implementation, the projected vector is needed to determine the pose data of the projected position point. Therefore, only the unit vector needs to be calculated, and there is no need to retain the specific length of the vector (because it is an indicator of direction, and in many cases, the vector pointing from the preset point to the initial target point often exceeds the preset space. In this case, the length of the vector projected from the preset point to the initial target point onto the base region may be the radius).

[0114] Then, based on the unit vector and the radius of the base region, the projected position point is determined; wherein, the projected position point is the position point located on the generatrix of the constrained space cone; Specifically, given a unit vector (which indicates direction), and combined with the radius of the base region (a circular plane), a planar point can be determined. This planar point lies on the edge of the base region (i.e., on the circumference). The vector pointing from the preset point to this planar point is also the vector projected from the preset point to the initial target point onto the boundary surface of the constrained space.

[0115] More specifically, if the projected position point is located between the preset point and the plane point, then the projected position point is directly used as the corrected position point; if the projected position point exceeds the plane point, then the plane point is directly used as the corrected position point.

[0116] Understandably, this method can be used to correct the initial target point.

[0117] Finally, based on the projected position points and the projection relationship, the pose data of the projected position points are determined.

[0118] As described above, after determining the projected position point (position data), and combining the projection relationship (by transforming the initial target point's pose data through projection), the pose data of the projected position point can be determined.

[0119] In this embodiment, by projecting the initial target point along the vector direction pointing from the preset point onto the boundary surface (generatrix) of the conical constraint space, efficient correction of the end-effector's trajectory is achieved. Its advantages lie in achieving a balance between geometric accuracy and computational efficiency. Specifically, by projecting the target point onto the unit vector direction of the base region and scaling it to the conical generatrix, the corrected pose is ensured to be strictly within the constraint space boundary. This prevents the tool from exceeding the safe zone (e.g., intruding into dangerous areas within the patient's body) and simplifies the projection process through vector operations (requiring only unit vector normalization and radius scaling), meeting real-time control requirements. Simultaneously, the rotational symmetry of the conical constraint space allows the projection logic to cover any rotation angle around the axis, providing consistent protection for multi-directional operations in different surgical procedures, ultimately maintaining operational smoothness while ensuring surgical safety.

[0120] In some implementations, the step of determining whether the intermediate pose data satisfies the second constraint condition includes: When the first data is within the first numerical range, firstly, based on the vector pointing from the preset point to the initial target point, the preset point, and the distance from the preset point to the base region, a first plane point is determined. The first plane point represents the location point where the vector pointing from the preset point to the initial target point passes through the base region. Then, the distance from the initial target point to the preset point and the distance from the first plane point to the preset point are calculated respectively. If the distance from the initial target point to the preset point is less than or equal to the distance from the first plane point to the preset point, it is determined that the second constraint condition is satisfied.

[0121] When the first data is within the second numerical range, firstly, based on the vector pointing from the preset point to the projected position point, the second plane point is determined. The second plane point is the position point where the vector pointing from the preset point to the projected position point passes through the base region. Then, the distance from the projected position point to the preset point and the distance from the second plane point to the preset point are calculated respectively. If the distance from the projected position point to the preset point is less than or equal to the distance from the second plane point to the preset point, it is determined that the second constraint condition is satisfied.

[0122] When the first data is in the third numerical range, the process of determining whether the intermediate pose data satisfies the second constraint condition is the same as the process when the first data is in the second numerical range.

[0123] When the first data is in the fourth numerical range, the process of determining whether the intermediate pose data meets the second constraint condition is the same as the process when the first data is in the first numerical range.

[0124] In this embodiment, a layered interval judgment and dynamic projection verification mechanism achieves precise control of the end-effector pose under dual constraints. Different verification logic can be designed for different rotation angle intervals (e.g., the first to fourth intervals). For example, in the first interval, the distance between the initial target point and the base region is directly compared, while in the second and third intervals, a secondary depth verification is performed based on the corrected projection point, ensuring accurate determination of the risk of exceeding the limit when approaching the constraint boundary from different angles. At the same time, by reusing the same logic to process symmetrical angle intervals (the third interval reuses the second interval, and the fourth interval reuses the first interval), the algorithm complexity and computational resource consumption are significantly reduced while ensuring safety. In addition, by calculating the dynamic distance from the end-effector to the projection point of the base region in real time, the surgical robot system can adaptively adjust the maximum allowable depth, avoiding over-constraint or under-constraint problems caused by static thresholds. This enables high-precision, low-latency real-time correction in complex surgical scenarios, comprehensively ensuring the safety of the patient's anatomical structures.

[0125] In some implementations, the step of correcting the intermediate pose data using a second constraint when the constraint is not met to obtain the second pose data includes: If the first data is within the first numerical range and the second constraint condition is not met, the pose data of the first plane point shall be used as the second pose data. When the first data is within the second numerical range, and when the second constraint condition is not met, the pose data of the second plane point is used as the second pose data. If the first data is in the third numerical range and the second constraint condition is not met, the pose data of the second plane point shall be used as the second pose data. If the first data is in the fourth numerical range and the second constraint condition is not met, the pose data of the first plane point shall be used as the second pose data.

[0126] This implementation achieves intelligent constraints on the end-effector pose through a sectional correction strategy. Its core benefits are reflected in three aspects: First, by employing a sectional differentiated processing mechanism (using the first plane point for correction in the first / fourth sectional and the second plane point for correction in the second / third sectional), it ensures that out-of-bounds behavior under different rotation angles receives optimal safety correction. This avoids robotic arm tremors or discontinuous movement caused by a single correction mode while maintaining the smoothness of surgical operations. Second, by dynamically selecting the projection points of the base region as correction targets (first / second plane points), the corrected pose strictly adheres to the preset geometric constraint space boundaries. This prevents loss of operational flexibility due to over-correction and eliminates safety hazards caused by insufficient correction. Finally, the design of using the same correction logic for symmetrical sectional (second and third, first and fourth) significantly improves the algorithm's execution efficiency.

[0127] In a specific implementation plan, such as Figure 2 As shown, in this embodiment, the constraint space is cone-shaped, with a circular plane in the base region. Rays emanating from a preset point (RCM) connect to the boundary of the circular plane, thus obtaining a cone-shaped constraint space.

[0128] In this implementation, the vector pointing directly from the preset point to the base region is the preset vector, which is also the normal vector of the base region pointing to the patient. The first constraint is rMax. The distance from the preset point to the base region is d1, and the radius of the base region is rv. T1 can be a position point in space indicated by the pose data of the current end effector.

[0129] First, the pose data T2 of the end effector at the next moment can be calculated according to the admittance control algorithm, and the position point in the space indicated by the pose data T2 is taken as P2.

[0130] Then, calculate the vector Z2 pointing from the preset point to the position point P2 of the end tool at the next moment, and calculate the angle a1 (first data) between the vector Z (preset vector) and the vector Z2 (the vector pointing from the preset point to the initial target point, which is also the axial vector of the end tool).

[0131] Next, the range of the included angle a1 is determined, and different corrections are performed based on the range of the included angle a1.

[0132] The first case, that is, when the first data point is within the first numerical range, is as follows: Figure 3 As shown, when the included angle a1 is less than or equal to rMax (the first constraint condition): Step 1: Keep the Y-axis of the tool coordinate system of the end effector unchanged, and calculate the Z-axis of the tool coordinate system of the pose data with vector Z2 as the next moment, and then calculate the pose matrix R of the end effector at the next moment.

[0133] Step 2: Calculate the position point P2 at the next moment, and let P4 = P2.

[0134] Step 3: Based on vector Z2, preset point, and distance d1, calculate the plane point P5 through which vector Z2 passes through the base region (that is, the first plane point, which is determined when the first data is in the first numerical range, based on the vector pointing from the preset point to the initial target point, the preset point, and the distance from the preset point to the base region).

[0135] Step 4: Calculate the distance d4 from position point P4 to the preset point and the distance d5 from plane point P5 to the preset point (that is, calculate the distance from the initial target point to the preset point and the distance from the first plane point to the preset point).

[0136] Step 5: Determine whether distance d4 is less than or equal to distance d5.

[0137] Step 6: If yes, set position point P4 as the position point of the end effector at the next moment; otherwise, set plane point P5 as the position point of the end effector at the next moment. Therefore, the pose matrix T3 of the end effector at the next moment can be obtained based on its position and attitude matrix R. (That is, when the first data is within the first numerical range and the second constraint condition is not met, the pose data of the first plane point is used as the second pose data; correspondingly, when the first data is within the first numerical range and the second constraint condition is met, the first pose data is directly used as the second pose data, i.e., no correction is required.) The second scenario, such as Figure 4As shown, when the included angle a1 is greater than rMax (the first constraint condition) and less than 90 degrees: Step 1: First, calculate the unit vector ZP2 projected from vector Z2 onto the base region (that is, calculate the unit vector projected from the preset point to the initial target point onto the base region). Then, calculate the plane point P3 based on the unit vector ZP2 and the radius rv. Finally, calculate the vector Z3 from the preset point to the plane point P3.

[0138] Step 2: Keep the Y-axis of the tool coordinate system of the end effector unchanged, and calculate the Z-axis of the tool coordinate system of the pose data with vector Z3 as the next moment, and then calculate the pose matrix R of the end effector at the next moment.

[0139] Step 3: Calculate the position point P4 projected from position point P2 onto line Z3 (that is, determine the projected position point based on the unit vector and the radius of the base region; wherein the projected position point is located on the generatrix of the constrained space cone). Step 4: Based on vector Z3, the preset point, and distance d1, calculate the plane point P5 through which vector Z3 passes in the base region (it can be understood that this plane point P5 is actually the plane point P3 mentioned above, which is also the second plane point).

[0140] Step 5: Calculate the distance d4 from position point P4 to the preset point, and the distance d5 from plane point P5 to the preset point.

[0141] Step 6: Determine whether distance d4 is less than or equal to distance d5.

[0142] Step 7: If yes, then position point P4 is taken as the position point of the end effector at the next moment; otherwise, position point P5 is taken as the position point of the end effector at the next moment. Therefore, the pose matrix T3 of the end effector at the next moment can be obtained based on the position point of the end effector at the next moment and the pose matrix R.

[0143] The above steps are as follows: First, calculate the unit vector projected from the preset point to the initial target point onto the base region; then, determine the projected position point based on the unit vector and the radius of the base region; wherein the projected position point is located on the generatrix of the constrained space cone; finally, determine the pose data of the projected position point based on the projected position point and the projection relationship.

[0144] Understandably, the above steps involve projecting the vector pointing from the preset point to the initial target point onto the boundary surface of the constrained space to obtain the pose data of the projected position point. In other words, if the projected position point is still on the boundary surface (e.g., position point P4), then the pose data of that position point is directly used as the second pose data. If it is not on the boundary surface, that is, it exceeds the boundary surface, then the position point is corrected, and the pose data of the position point on the boundary surface is used as the second pose data.

[0145] The third scenario, such as Figure 5 As shown in the right-hand diagram, when the included angle a1 is greater than or equal to 90 degrees and less than or equal to 180 degrees - rMax: Step 1: Directly invert vector Z2; Step 2: Proceed to Step 1 in the second scenario to perform the calculation.

[0146] The fourth case, such as Figure 5 As shown in the left-hand diagram, when the included angle a1 is greater than 180 degrees - rMax degrees and less than 180 degrees: Step 1: Directly invert vector Z2; Step 2: Proceed to Step 1 in the first case to perform the calculation.

[0147] In one specific implementation scheme, a method for correcting pose data is provided, which may include: Step 1: Set the coordinate system of the robotic arm's end effector at the current moment. The axis is , Unit vector, vector Direction downwards, set A point on the axis as Point, set the normal vector of the virtual plane to be Vector Direction downwards, set the virtual plane distance for planning. Point for Set the maximum rotation angle ; Step 2: Calculate the radius of the virtual plane as follows: Step 3: Obtain the current position of the robotic arm's end effector. Based on the admittance control algorithm, the tool pose at the next moment is calculated. Let the position of the tool at the next moment be point A. ; Step 4, Calculation Point to the tool's position at the next moment. vector Calculate vector and vector The angle between The formula is: Step 5: Determine the included angle The range, if Then proceed to step 6; if Then proceed to step 9; if Then the vector After inverting, proceed to step 6; if Then the vector After inverting, proceed to step 9; Step 6, Robotic Arm End-Edge Tool Coordinate System The axis remains unchanged, with The axis represents the next moment. Axis, will shaft and The cross product of axes is normalized to calculate the end-effector attitude matrix at the next time step. The calculation formula is: Step 7: Calculate the position at the next time step. ;if If yes, proceed to step 8; otherwise, proceed to step 17. Step 8: Based on the vector , Point, distance ,calculate Passing through virtual plane points Proceed to step 15; the calculation formula is: Step 9: Calculate the vector The unit vector projected onto the virtual plane is The calculation formula is: Step 10, according to and radius Calculation points The calculation formula is: Step 11, Calculation Point to point vector The calculation formula is: Step 12, Robotic Arm End-Edge Tool Coordinate System The axis remains unchanged, with The axis represents the next moment. Axis, will shaft and Cross product of axes to calculate the end-effector attitude matrix at the next time step. ; Step 13, Calculate the points Projected onto a straight line Points on ;if If the result is positive, proceed to step 14; otherwise, proceed to step 17. The calculation formula is: Step 14: Based on the vector , Point, distance ,calculate Passing through virtual plane points The calculation formula is: Step 15, Calculation Click Distance between points , Click Distance between points The calculation formula is: Step 16, if If yes, proceed to step 17; otherwise, proceed to step 18. The point represents the tool's position at the next moment. Proceed to step 18; Step 17, with The point represents the tool's position at the next moment. ; Step 18: Based on the tool's position at the next moment and tool posture The tool pose matrix is ​​obtained at the next moment. The formula is: According to an embodiment of the present invention, an electronic device is provided; please refer to... Figure 6 The electronic device in this embodiment may include one or more of the following components: a processor, a network interface, memory, non-volatile memory, and one or more application programs, wherein the one or more application programs may be stored in non-volatile memory and configured to be executed by one or more processors, and the one or more programs are configured to perform the methods as described in the foregoing method embodiments.

[0148] According to embodiments of the present invention, a computer-readable storage medium is provided having a computer program stored thereon, which, when executed by a computer, causes the computer to perform the method described in any of the above embodiments.

[0149] According to an embodiment of the present invention, a computer program product containing instructions is also provided, which, when executed by a computer, cause the computer to perform a switch switching strategy execution method according to any of the above embodiments.

[0150] It should be noted that the terms "first," "second," etc., in the specification, claims, and accompanying drawings of this application are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate so that the embodiments of this application described herein can be implemented in orders other than those illustrated or described herein. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover non-exclusive inclusion; for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0151] Optionally, specific examples in this embodiment can refer to the examples described in the above embodiments, and will not be repeated here.

[0152] The sequence numbers of the embodiments in this application are for descriptive purposes only and do not represent the superiority or inferiority of the embodiments.

[0153] In the above embodiments of this application, the descriptions of each embodiment have different focuses. For parts not described in detail in a certain embodiment, please refer to the relevant descriptions of other embodiments.

[0154] The above description is only a preferred embodiment of this application. It should be noted that for those skilled in the art, several improvements and modifications can be made without departing from the principle of this application, and these improvements and modifications should also be considered within the scope of protection of this application.

Claims

1. A method for correcting pose data, characterized in that, The method includes: When a preset trigger condition is detected, a first pose data is generated based on a preset motion control algorithm; wherein, the first pose data represents the initial target pose data of the robotic arm end effector. Based on the spatial vector relationship between the initial target pose data and the preset point, the first pose data is corrected to obtain the second pose data; wherein, the second pose data is pose data that satisfies the first constraint condition and the second constraint condition; wherein, the first constraint condition is a constraint condition that enables the end-effector to avoid excessive rotation, thereby preventing the end-effector from damaging the protected object; the second constraint condition is a constraint condition that enables the end-effector to avoid excessive penetration, thereby preventing the end-effector from damaging the protected object.

2. The method according to claim 1, characterized in that, The preset triggering conditions include: The end-effector enters the preset area of ​​the preset point; The force data experienced by the end effector exceeds the preset force data threshold; Received a user-input command to switch motion modes; The end-effector's movement time exceeds a preset time threshold; or, The end effector's movement speed exceeds a preset speed threshold.

3. The method according to claim 1, characterized in that, The initial target pose data indicates an initial target point existing in space; the first constraint condition includes a constraint condition that constrains the maximum deviation angle between the axial vector of the end tool and a preset vector, and the second constraint condition includes a constraint condition that constrains the maximum distance from the end of the end tool to the preset point; wherein, the axial vector of the end tool is a vector representing the distance from the end of the end tool near the robotic arm to the end far from the robotic arm; The step of correcting the first pose data based on the spatial vector relationship between the initial target pose data and the preset point to obtain the second pose data includes: Based on the spatial vector relationship between the initial target point and the preset point, first data and second data are calculated; wherein, the first data represents the angle between the vector pointing from the preset point to the initial target point and the preset vector; and the second data represents the distance data from the initial target point to the preset point. Determine whether the first data and the second data satisfy the first constraint condition corresponding to the first data and the second constraint condition corresponding to the second data, respectively. If at least one constraint is not satisfied, the pose data of the initial target point is modified based on the first constraint and / or the second constraint to obtain the second pose data.

4. The method according to claim 1, characterized in that, The initial target pose data indicates an initial target point existing in space; the first constraint condition includes a constraint condition for the maximum deviation angle between the axial vector of the end tool and the preset vector; the second constraint condition also includes a constraint condition for the maximum dynamic distance from the end of the end tool to the preset point when the end tool is rotated to different rotation angles; wherein, the axial vector of the end tool is a vector representing the distance from the end of the end tool near the robot arm to the end far from the robot arm; The step of correcting the first pose data based on the spatial vector relationship between the initial target pose data and the preset point to obtain the second pose data includes: Based on the spatial vector relationship between the initial target point and the preset point, first data is calculated; wherein, the first data represents the angle between the vector pointing from the preset point to the initial target point and the preset vector; Determine the numerical range in which the first data falls; wherein the numerical range is a numerical range divided based on the first constraint condition; Based on the numerical range in which the first data is located, the first pose data is corrected to obtain intermediate pose data; Determine whether the intermediate pose data satisfies the second constraint condition; If the conditions are not met, the intermediate pose data is corrected using the second constraint to obtain the second pose data.

5. The method according to claim 4, characterized in that, The numerical range includes a first numerical range, a second numerical range, a third numerical range, and a fourth numerical range; wherein, the first numerical range represents the range from zero degrees to the maximum deviation angle, the second numerical range represents the range from the maximum deviation angle to ninety degrees, the third numerical range represents the range from ninety degrees to one hundred and eighty degrees minus the maximum deviation angle, and the fourth numerical range represents the range from one hundred and eighty degrees minus the maximum deviation angle to one hundred and eighty degrees. The step of correcting the first pose data based on the numerical range in which the first data falls to obtain intermediate pose data includes: When the first data is within the first numerical range, the first pose data is used as the intermediate pose data; When the first data is within the second numerical range, the vector pointing from the preset point to the initial target point is projected onto the boundary surface of the constraint space to obtain the pose data of the projected position point; wherein, the constraint space is a constraint space with the preset point as the vertex, a base region at a preset distance from the preset point as the bottom surface, and the boundary surface connecting the vertex and the bottom surface; wherein, the base region is set between the preset point and the protected object; When the first data is in the third numerical range, firstly, the vector projection pointing from the preset point to the initial target point is inverted, and then the inverted vector is projected onto the boundary surface of the constraint space to obtain the pose data of the projected position point. When the first data is in the fourth numerical range, firstly, the vector projection pointing from the preset point to the initial target point is inverted, and then the pose data of the position point indicated by the inverted vector is used as the intermediate pose data.

6. The method according to claim 5, characterized in that, The base region is a plane, a curved surface, an irregular surface with at least some planes, or an irregular surface with at least some curved surfaces.

7. The method according to claim 5, characterized in that, When the base region is a circular plane and the constraint space is conical, the step of projecting the vector pointing from the preset point to the initial target point onto the boundary surface of the constraint space to obtain the pose data of the projected position point includes: Calculate the unit vector projected from the preset point to the initial target point onto the base region; The projected position point is determined based on the unit vector and the radius of the base region; wherein the projected position point is located on the generatrix of the constrained space cone. Based on the projected position points and the projection relationship, the pose data of the projected position points are determined.

8. The method according to claim 7, characterized in that, The step of determining whether the intermediate pose data satisfies the second constraint condition includes: When the first data is within the first numerical range, firstly, based on the vector pointing from the preset point to the initial target point, the preset point, and the distance from the preset point to the base region, a first plane point is determined. The first plane point represents the location point where the vector pointing from the preset point to the initial target point passes through the base region. Then, the distance from the initial target point to the preset point and the distance from the first plane point to the preset point are calculated respectively. If the distance from the initial target point to the preset point is less than or equal to the distance from the first plane point to the preset point, it is determined that the second constraint condition is satisfied. When the first data is within the second numerical range, firstly, based on the vector pointing from the preset point to the projected position point, the second plane point is determined. The second plane point represents the position point where the vector pointing from the preset point to the projected position point passes through the base region. Then, the distance from the projected position point to the preset point and the distance from the second plane point to the preset point are calculated respectively. If the distance from the projected position point to the preset point is less than or equal to the distance from the second plane point to the preset point, it is determined that the second constraint condition is satisfied. When the first data is in the third numerical range, the process of determining whether the intermediate pose data satisfies the second constraint condition is the same as the process when the first data is in the second numerical range. When the first data is in the fourth numerical range, the process of determining whether the intermediate pose data meets the second constraint condition is the same as the process when the first data is in the first numerical range.

9. The method according to claim 8, characterized in that, The step of correcting the intermediate pose data using a second constraint when the constraint is not met, to obtain the second pose data, includes: If the first data is within the first numerical range and the second constraint condition is not met, the pose data of the first plane point shall be used as the second pose data. When the first data is within the second numerical range, and when the second constraint condition is not met, the pose data of the second plane point is used as the second pose data. If the first data is in the third numerical range and the second constraint condition is not met, the pose data of the second plane point shall be used as the second pose data. If the first data is in the fourth numerical range and the second constraint condition is not met, the pose data of the first plane point shall be used as the second pose data.

10. An electronic device, characterized in that, include: A memory, and one or more processors communicatively connected to the memory; The memory stores instructions that can be executed by the one or more processors to cause the one or more processors to implement the method as described in any one of claims 1 to 9.

Images