Mechanical arm trajectory planning method, device and equipment and medium

Abstract

The invention relates to the technical field of mechanical arm control, and provides a mechanical arm trajectory planning method and device, equipment and a medium. The method comprises the steps that the pose of an obstacle bounding box under a robot coordinate system is fitted; fitting a robot bounding box; taking the center point of the tool at the tail end of the operation arm as a starting point, taking the center point of the tool at the tail end of the conveying arm as an ending point, and combining the to-be-fitted track length and the obstacle avoidance space to generate a motion track curve of the operation arm; track point positions are extracted from the motion track curve, the posture of the operation arm at each track point position is planned, and the posture of the operation arm at the corresponding track point position is adjusted under the condition that whether the track point positions of the operation arm are reachable or not is verified; and under the reachable condition, the track point location of the operation arm is matched with the current endoscope conveying length, and the track point location of the operation arm is described through the pose of the operation arm. According to the invention, through environment perception, kinematics modeling and trajectory generation, accurate obstacle avoidance is realized in a complex and changeable operation environment, and the safety and reliability are improved.

Description

Technical Field

[0001] This application relates to the technical field of robotic arm control, and particularly to a method, device, equipment and medium for robotic arm trajectory planning. Background Art

[0002] Natural cavities such as the digestive tract and respiratory tract are common sites for human diseases. Since the lesions are inside the natural cavities of the human body, a flexible endoscope is required for examination.

[0003] Traditional flexible endoscope examinations or surgeries require a person to use both hands to hold the endoscope body, operate the knobs, and manually insert the endoscope body to complete the operation process. Some examinations or surgeries need to be carried out under the guidance of radiographic images, and medical staff need to wear heavy lead protective clothing and perform manual operations on the flexible endoscope for a long time, which has a great impact on the quality, physical strength and health of medical staff during diagnosis and treatment operations. It has a greater restriction on experienced older or female medical staff, and even forces them to give up endoscopic work.

[0004] With the development of robot-assisted technology, doctors can adjust the delivery length and posture of the flexible endoscope by adjusting the handle switch and buttons, which greatly reduces the physical strength and manual operation fatigue of doctors, reduces the operation requirements of the surgery, reduces the radiation to medical staff, and at the same time improves the interaction between medical staff and images.

[0005] However, the flexible endoscope control robot system in the related technology still has significant defects. In the endoscope insertion link, the robot needs to plan the trajectory of the operating arm in advance according to the position of the patient's natural orifice to prevent damage to the endoscope or the actuator due to improper operation. During actual surgery, the environment is complex and changeable, and the established trajectory planning method is difficult to ensure that the robot can accurately avoid obstacles, and its reliability in ensuring safe movement is significantly insufficient. Summary of the Invention

[0006] This application provides a method, device, equipment and medium for robotic arm trajectory planning, which solves the problem that the established trajectory planning method is difficult to ensure that the robot can accurately avoid obstacles and has insufficient reliability in ensuring safe movement.

[0007] To solve the above technical problems, the technical solutions provided in this application are as follows:

[0008] In a first aspect, this application provides a method for robotic arm trajectory planning, which is applied to a flexible endoscope control robot system. The flexible endoscope control robot system includes a delivery arm and an operating arm. The method includes:

[0009] Obtain the point cloud data of obstacles in the surgical environment, and fit the pose of the obstacle bounding box in the robot coordinate system;

[0010] Obtain the poses of the center point of the end effector of the manipulator and the center point of the end effector of the delivery arm in the robot coordinate system, and fit the robot bounding box in combination with the robot structure parameters;

[0011] Based on the current endoscope delivery length, obtain the length of the trajectory to be fitted. Based on the pose of the obstacle bounding box in the robot coordinate system, obtain the obstacle avoidance space. Taking the center point of the end effector of the manipulator as the starting point and the center point of the end effector of the delivery arm as the ending point, and combining the length of the trajectory to be fitted and the obstacle avoidance space, generate the motion trajectory curve of the manipulator;

[0012] Extract trajectory points from the motion trajectory curve and plan the poses of the manipulator at each of the trajectory points;

[0013] Verify whether the trajectory points of the manipulator are reachable. In the case where the trajectory points of the manipulator are not reachable, adjust the pose of the manipulator at the corresponding trajectory points. In the case where the trajectory points of the manipulator are reachable, the trajectory points of the manipulator are matched by the current endoscope delivery length, and the trajectory points of the manipulator are described by the pose of the manipulator.

[0014] In some possible implementation manners, the obtaining the point cloud data of the obstacles in the surgical environment and fitting the pose of the obstacle bounding box in the robot coordinate system includes:

[0015] Collect the image information of the obstacles in the surgical environment, and obtain the point cloud data of the obstacles in the surgical environment through image matching and three-dimensional reconstruction techniques;

[0016] Based on the obtained point cloud data of the obstacles in the surgical environment, use the bounding box algorithm to fit the pose of the obstacle bounding box in the robot coordinate system, and the bounding box algorithm includes at least one of an axis-aligned bounding box and an oriented bounding box.

[0017] In some possible implementation manners, the flexible endoscope manipulation robot system further includes a delivery length sensor installed on the delivery arm, and the obtaining the length of the trajectory to be fitted based on the current endoscope delivery length includes:

[0018] Obtain the current endoscope delivery length through the delivery length sensor;

[0019] Combine the current endoscope delivery length and the endoscope length parameter to obtain the length of the trajectory to be fitted.

[0020] In some possible implementation manners, taking the center point of the end effector of the manipulator as the starting point and the center point of the end effector of the delivery arm as the ending point, and combining the length of the trajectory to be fitted and the obstacle avoidance space, generating the motion trajectory curve of the manipulator includes:

[0021] The motion trajectory curve is described by a B-spline curve, and the B-spline curve is solved through an optimization model. The objective function of the optimization model is:

[0022]

[0023] Where:

[0024] —Control points affecting the current curve value;

[0025] —Knot vector;

[0026] —Square integral of the second derivative of the curve;

[0027] —B-spline curve;

[0028] —Weight coefficient;

[0029] —Knot;

[0030] —Total length of the endoscope;

[0031] —Current endoscope delivery length;

[0032] The constraint conditions of the optimization model include:

[0033]

[0034]

[0035]

[0036] Where:

[0037] —Kth B-spline basis function;

[0038] —Starting point;

[0039] —End point;

[0040] —Obstacle bounding box;

[0041] —Collision avoidance safety distance.

[0042] In some possible implementation manners, extracting trajectory points from the motion trajectory curve includes:

[0043] The motion trajectory curve is equally divided by unit length, and the equal division points are the trajectory points.

[0044] In some possible implementation manners, verifying whether the trajectory point of the operating arm is reachable includes:

[0045] Verifying whether the first constraint condition and the second constraint condition are simultaneously satisfied. When the first constraint condition and the second constraint condition are simultaneously satisfied, the trajectory point of the operating arm is reachable; when the first constraint condition and the second constraint condition are not simultaneously satisfied, the trajectory point of the operating arm is unreachable;

[0046] The first constraint condition is that when moving from the previous adjacent trajectory point to the trajectory point, the rotation angles of the joints of the operating arm do not exceed the rotation angle range;

[0047] The second constraint condition is that at the trajectory point, the robot bounding box and the obstacle bounding box do not interfere.

[0048] In some possible implementation manners, the method further includes:

[0049] When the obstacle bounding box enters the working space of the operating arm, and the pose of the obstacle bounding box changes, and the obstacle bounding box is on the motion trajectory curve, the motion trajectory curve is regenerated, and the operating arm switches to the action of the new motion trajectory curve.

[0050] In a second aspect, the present application provides a robotic arm trajectory planning device, which is applied to a flexible endoscope manipulation robot system. The flexible endoscope manipulation robot system includes a conveying arm and an operating arm; the device includes:

[0051] A first fitting unit, configured to obtain the point cloud data of the obstacles in the surgical environment and fit the pose of the obstacle bounding box in the robot coordinate system;

[0052] A second fitting unit, configured to obtain the poses of the tool center point at the end of the operating arm and the tool center point at the end of the conveying arm in the robot coordinate system, and fit the robot bounding box in combination with the robot structure parameters;

[0053] A generating unit, configured to obtain the length of the trajectory to be fitted based on the current endoscope conveying length, obtain the obstacle avoidance space based on the pose of the obstacle bounding box in the robot coordinate system, use the tool center point at the end of the operating arm as the starting point, use the tool center point at the end of the conveying arm as the ending point, and generate the motion trajectory curve of the operating arm in combination with the length of the trajectory to be fitted and the obstacle avoidance space;

[0054] A planning unit is used to extract trajectory points from the motion trajectory curve and plan the poses of the manipulator at each of the trajectory points;

[0055] A verification unit is used to verify whether the trajectory points of the manipulator are reachable. In the case where the trajectory points of the manipulator are unreachable, adjust the pose of the manipulator at the corresponding trajectory points; in the case where the trajectory points of the manipulator are reachable, the trajectory points of the manipulator are matched by the current endoscope delivery length, and the trajectory points of the manipulator are described by the pose of the manipulator.

[0056] In some possible implementation manners, the first fitting unit includes:

[0057] A first acquisition subunit is used to collect image information of obstacles in the surgical environment and obtain the point cloud data of the obstacles in the surgical environment through image matching and three-dimensional reconstruction technologies;

[0058] A first fitting subunit is used to fit the pose of the obstacle bounding box in the robot coordinate system based on the obtained point cloud data of the obstacles in the surgical environment, and the bounding box algorithm includes at least one of an axis-aligned bounding box and an oriented bounding box.

[0059] In a third aspect, the present application provides a computing device, which includes a processor and a memory. Instructions are stored in the memory, and the processor executes the instructions to enable the computing device to execute the manipulator trajectory planning method as described in the first aspect or any implementation manner of the first aspect of the present application.

[0060] In a fourth aspect, the present application provides a computer-readable storage medium, in which computer-readable instructions are stored. When the computer-readable instructions run on a computing device, the computing device is enabled to execute the manipulator trajectory planning method as described in the first aspect or any implementation manner of the first aspect above.

[0061] Based on the implementation manners provided in the above aspects, the present application can be further combined to provide more implementation manners.

[0062] Based on the above description, it can be seen that the technical solution of the present application has the following beneficial effects:

[0063] In the embodiments of the present application, first, by acquiring the point cloud data of the obstacles in the surgical environment and fitting the pose of the obstacle bounding box in the robot coordinate system, the obstacles in the surgical environment are accurately identified and located. This step provides accurate environmental information for subsequent trajectory planning, enabling the robot to effectively avoid obstacles when planning the motion trajectory and preventing the robot from colliding with the obstacles. Secondly, by using the forward kinematics of the robot, the poses of the center points of the tools at the end of the operating arm and the center points of the tools at the end of the delivery arm in the robot coordinate system are determined, and the robot bounding box is fitted in combination with the robot structure parameters to accurately describe the motion range and pose of the robot, providing the kinematic information of the robot itself for trajectory planning, enabling the robot to better adapt to different surgical environments and operation requirements, and improving the flexibility and adaptability of the system. On this basis, in the embodiments of the present application, the length of the trajectory to be fitted is obtained based on the current endoscope delivery length, and the obstacle avoidance space is obtained based on the pose of the obstacle bounding box in the robot coordinate system. Starting from the center point of the tool at the end of the operating arm and ending at the center point of the tool at the end of the delivery arm, combining the length of the trajectory to be fitted and the obstacle avoidance space, a motion trajectory curve of the operating arm is generated. This step generates a motion trajectory that not only meets the operation requirements but also avoids obstacles by comprehensively considering the endoscope delivery length and the obstacle avoidance space, significantly improving the reliability and safety of trajectory planning. Further, in the embodiments of the present application, the trajectory points are extracted from the motion trajectory curve, the poses of the operating arm at each trajectory point are planned, and whether the trajectory points of the operating arm are reachable is verified. In the case where the trajectory points of the operating arm are not reachable, by adjusting the pose of the operating arm at the corresponding trajectory point, it is ensured that the operating arm can smoothly reach the predetermined trajectory points. This step further improves the feasibility and reliability of trajectory planning, ensuring that the robot can safely and accurately complete the endoscope delivery task according to the predetermined trajectory during actual operation. In the case where the trajectory points of the operating arm are reachable, the trajectory points of the operating arm are matched by the current endoscope delivery length, ensuring the accurate positioning of the operating arm. The trajectory points of the operating arm are described by the pose of the operating arm, so that the pose of the operating arm at each trajectory point is determined.

[0064] In summary, the robotic arm trajectory planning method provided by the embodiments of the present application can achieve precise obstacle avoidance in a complex and changing surgical environment through accurate environmental perception, kinematic modeling, and trajectory generation, significantly improving the operation safety and reliability of the flexible endoscope manipulation robot system. BRIEF DESCRIPTION OF THE DRAWINGS

[0065] In combination with the accompanying drawings and with reference to the following specific embodiments, the above and other features, advantages, and aspects of the embodiments of the present application will become more apparent. Throughout the drawings, the same or similar reference numerals represent the same or similar elements. It should be understood that the drawings are schematic, and the original components and elements are not necessarily drawn to scale.

[0066] Figure 1a Schematic diagram of a flexible endoscope provided by an embodiment of the present application;

[0067] Figure 1b Structural schematic diagram of a flexible endoscope operating robot system provided by an embodiment of the present application;

[0068] Figure 2 Flowchart of a specific embodiment of the robotic arm trajectory planning method provided by the present application;

[0069] Figure 3 Structural schematic diagram of a specific embodiment of the robotic arm trajectory planning device provided by the present application.

[0070] Among them, Figure 1a - Figure 3 The reference numerals are as follows:

[0071] 1 - Flexible endoscope; 11 - Operating part; 110 - Endoscope workstation interface; 111 - Large dial; 112 - Small dial; 113 - Function button; 114 - Suction valve button; 115 - Water vapor valve button; 116 - Instrument channel; 12 - Insertion part; 13 - Tip part;

[0072] 100 - Robot trolley; 101 - Conveyor arm; 102 - Operating arm; 103 - Conveyor device; 104 - Operating device; 200 - Endoscope workstation; 300 - Instrument operation table; 400 - Operating table; 500 - Binocular camera; 600 - Robotic arm trajectory planning device; 601 - First fitting unit; 602 - Second fitting unit; 603 - Generation unit; 604 - Planning unit; 605 - Verification unit. Detailed implementation manners

[0073] The embodiments of the present application will be described in more detail below with reference to the accompanying drawings. Although some embodiments of the present application are shown in the drawings, it should be understood that the present application can be implemented in various forms and should not be construed as limited to the embodiments set forth herein. On the contrary, these embodiments are provided to more thoroughly and completely understand the present application. It should be understood that the drawings and embodiments of the present application are only for exemplary purposes and are not used to limit the protection scope of the present application.

[0074] The term "including" and its variations used herein are open-ended, that is, "including but not limited to". The term "based on" is "at least partially based on". The term "one embodiment" means "at least one embodiment"; the term "another embodiment" means "at least one additional embodiment"; the term "some embodiments" means "at least some embodiments". The relevant definitions of other terms will be given in the following description.

[0075] It should be noted that the concepts such as "first", "second", etc. mentioned in this application are only used to distinguish different devices, modules or units, and are not used to limit the order of functions performed by these devices, modules or units or their interdependent relationships.

[0076] It should be noted that the modifications of "one" and "multiple" mentioned in this application are illustrative rather than restrictive. Those skilled in the art should understand that unless clearly specified otherwise in the context, it should be understood as "one or more".

[0077] Natural cavities such as the digestive tract and respiratory tract are common sites for human diseases. Since the lesions are inside the natural cavities of the human body, soft endoscopes are required for examination.

[0078] For traditional soft endoscope examinations or surgeries, manual operations such as holding the endoscope body with both hands, operating the knobs, and manually transporting the endoscope body are required to complete the operation process. Some examinations or surgeries need to be carried out under the image guidance of radiation, and medical staff need to wear heavy lead protective clothing and other equipment to manually operate the soft endoscope for a long time, which has a great impact on the quality, physical strength and health of medical staff during diagnosis and treatment operations. In particular, it restricts more experienced older or female medical staff, and even forces them to give up endoscopic work.

[0079] With the development of robot-assisted technology, doctors can adjust the delivery length and posture of the soft endoscope by adjusting the handle switch and buttons, which greatly reduces the physical strength and manual operation fatigue of doctors, reduces the operation requirements of the surgery, reduces the radiation to medical staff, and at the same time can improve the interaction between medical staff and images.

[0080] However, the existing soft endoscope manipulation robot system still has significant defects. In the endoscope delivery link, the robot needs to plan the trajectory of the operating arm in advance according to the position of the patient's natural orifice to prevent damage to the endoscope or the actuator due to improper operation. During actual surgeries, the environment is complex and changeable, and the established trajectory planning method is difficult to ensure that the robot can accurately avoid obstacles, and its reliability in ensuring safe movement is significantly insufficient.

[0081] Based on this, the embodiment of the present application provides a robot arm trajectory planning method, which is applied to a soft endoscope manipulation robot system, aiming to solve the reliability and safety problems of the trajectory planning of the operating arm in the mirror delivery link of the soft endoscope manipulation robot system in the related technology. Specifically, the embodiment of the present application first obtains the point cloud data of the obstacles in the surgical environment and fits the position and posture of the obstacle bounding box in the robot coordinate system to accurately identify and locate the obstacles in the surgical environment. This step provides accurate environmental information for subsequent trajectory planning, so that the robot can effectively avoid obstacles when planning the motion trajectory, and avoid damage to the soft endoscope due to improper operation; secondly, the position and posture of the tool center point at the end of the operating arm and the tool center point at the end of the delivery arm in the robot coordinate system are determined by the robot's positive kinematics, and the robot bounding box is fitted in combination with the robot's structural parameters to accurately describe the robot's motion range and posture, and provide the robot's own kinematic information for trajectory planning, so that the robot can better adapt to different surgical environments and operation requirements, and improve the flexibility and adaptability of the system; on this basis, the embodiment of the present application obtains the length of the trajectory to be fitted based on the current endoscope delivery length, and obtains the length of the trajectory to be fitted based on the obstacle bounding box in the robot coordinate system. The obstacle avoidance space is obtained by taking the center point of the tool at the end of the operating arm as the starting point and the center point of the tool at the end of the conveying arm as the ending point. The motion trajectory curve of the operating arm is generated by combining the length of the trajectory to be fitted and the obstacle avoidance space. This step generates a motion trajectory that meets the operation requirements and avoids obstacles while ensuring safety by comprehensively considering the endoscope conveying length and the obstacle avoidance space, thereby significantly improving the reliability and safety of trajectory planning. Furthermore, the embodiment of the present application also extracts trajectory points from the motion trajectory curve, plans the posture of the operating arm at each trajectory point, and verifies whether the trajectory point of the operating arm is reachable. When the trajectory points of the manipulator arm are unreachable, the posture of the manipulator arm at the corresponding trajectory points is adjusted to ensure that the manipulator arm can smoothly reach the predetermined trajectory points. This step further improves the feasibility and reliability of trajectory planning, ensuring that the robot can safely and accurately complete the operation tasks according to the predetermined trajectory in actual operation. When the trajectory points of the manipulator arm are reachable, the trajectory points of the manipulator arm are matched by the current endoscope delivery length to ensure the accurate positioning of the manipulator arm. The trajectory points of the manipulator arm are described by the posture of the manipulator arm, so that the posture of the manipulator arm at each trajectory point is determined.

[0082] To sum up, the robot arm trajectory planning method provided in the embodiment of the present application can achieve precise obstacle avoidance in complex and changeable surgical environments through precise environmental perception, kinematic modeling and trajectory generation, and significantly improve the operational safety and reliability of the flexible endoscope control robot system.

[0083] It is understandable that all the defects existing in the above solutions are the results obtained by the applicant after practice and careful research. Therefore, the process of discovering the above problems and the solutions proposed for the above problems in the embodiments of the present application should be the contributions made by the applicant to the embodiments of the present application during the process of the present application.

[0084] To facilitate the understanding of the robotic arm trajectory planning method provided by the embodiments of the present application, the following will be described in conjunction with Figure 1a and Figure 1b for illustration. Figure 1a FIG. is a schematic structural diagram of a traditional flexible endoscope provided by an embodiment of the present application, Figure 1b FIG. is a schematic structural diagram of a flexible endoscope operating robot system provided by an embodiment of the present application.

[0085] In the embodiments of the present application, taking the flexible endoscope as a digestive flexible endoscope as an example, as Figure 1a shown, the flexible endoscope 1 includes an operation part 11, an insertion part 12, and a distal end part 13. The operation part 11 includes an endoscope workstation interface 110, a large dial 111, a small dial 112, function buttons 113, a suction valve button 114, a water-vapor valve button 115, and an instrument channel 116. When transporting the flexible endoscope 1, first control the distal end part 13 to insert into the patient's oral cavity, and then control the insertion part 12 to insert into the patient's oral cavity.

[0086] Combined with Figure 1a , taking the digestive flexible endoscope as an example, the specific method of manually operating the digestive flexible endoscope in the traditional way is that the medical staff holds the front end of the flexible endoscope with the right hand and holds the operation part 11 of the flexible endoscope with the left hand in front of the chest, adjusts the large and small knobs with the thumb, middle finger, and ring finger, controls the injection of air and water with the middle finger, and controls the suction valve button 114 with the index finger. When controlling the advancement and retreat of the flexible endoscope, it is necessary to rotate the mirror body to always maintain a good view. When reaching the lesion or the place where biopsy is needed, the corresponding instrument is inserted through the instrument channel, through the instrument channel inside the insertion part 12, to the distal end part 13. With the cooperation of the view under the endoscope, the corresponding operation is completed.

[0087] Combined with Figure 1b, A flexible endoscope operation robot system, comprising a robot, an endoscope workstation 200, an instrument operation table 300 and an operating table 400. The robot includes a robot trolley 100, a conveying arm (also known as the main arm) 101, an operating arm (also known as the slave arm) 102, a conveying device 103 and an operating device 104. The conveying arm 101 and the operating arm 102 are collectively referred to as robotic arms. Among them, both the conveying arm 101 and the operating arm 102 are multi-degree-of-freedom robotic arms. In the flexible endoscope operation robot system, a tool is usually fixed at the end of the robotic arm. For example, the operating device 104 is fixed at the end of the operating arm 102, and the operating part 11 of the flexible endoscope 1 is fixed on the operating device 104. The conveying device 103 is fixed at the end of the conveying arm 101. The operating device 104 and the operating part 11 of the flexible endoscope 1 can be regarded as a tool fixed at the end of the operating arm 102 together, and the conveying device 103 can be regarded as a tool fixed at the end of the conveying arm 101. The tool is also called an end effector.

[0088] In the master-slave control mode, the doctor's operation commands can be converted into the movements of the conveying arm 101, the operating arm 102, the conveying device 103 and the operating device 104. Among them, the operating device 104 operates the flexible endoscope 1 to bend, and the conveying device 103 controls the length of the flexible endoscope 1 entering the human body. Both of them control the rotation of the flexible endoscope 1 at the same time, or can be controlled separately, and the other follows. At the same time, the operating device 104 can drive the instruments supporting the flexible endoscope.

[0089] To facilitate the understanding of the present application, a robotic arm trajectory planning method provided by an embodiment of the present application will be described below with reference to the accompanying drawings.

[0090] See Figure 2 , Figure 2 is a flowchart of a specific embodiment of the robotic arm trajectory planning method provided by the present application.

[0091] The robotic arm trajectory planning method of the embodiment of the present application is applied to a flexible endoscope control robot system. The flexible endoscope control robot system includes a conveying arm and an operating arm. The method may include S201 - S205:

[0092] S201: Obtain the point cloud data of the obstacles in the surgical environment, and fit the pose of the obstacle bounding box in the robot coordinate system.

[0093] In Figure 1bIn the illustrated embodiment, a binocular camera 500 is used to photograph obstacles existing in the surgical environment, such as the endoscopic workstation 200, the instrument operation table 300, etc. The binocular camera 500 collects image information of the obstacles in the surgical environment. Based on the image information of the obstacles in the surgical environment, point cloud data of the obstacles in the surgical environment is obtained through image matching and three-dimensional reconstruction techniques. The point cloud data contains the three-dimensional coordinate information of many discrete points on the surface of the obstacles. Then, a bounding box algorithm, such as the axis-aligned bounding box algorithm or the oriented bounding box algorithm, is used to calculate the smallest bounding box that can completely enclose the obstacles according to the distribution range of the point cloud data. The bounding box has clear position and attitude information in the robot coordinate system, providing basic data for subsequent obstacle avoidance planning.

[0094] For example, for the instrument operation table 300, the minimum boundary coordinate of its bounding box in the X-axis direction, the minimum position , the maximum position in the X-axis direction , the minimum position in the Y-axis direction , the maximum position in the Y-axis direction are obtained through the algorithm. Since its spatial shape is regular, it is fitted into a cuboid with the Z-axis in the same direction as the Z-axis of the robot coordinate system and known side lengths , where:

[0095] ——The rotation matrix of the bounding box of the instrument operation table rotated by angle;

[0096] ——The initial position point set of the bounding box of the instrument operation table based on the robot coordinate system;

[0097] ——The zero-position offset of the bounding box of the instrument operation table, determining the spatial position of the bounding box of the instrument operation table in the robot coordinate system.

[0098] Among them, the binocular camera 500, also known as a stereo camera, is a device that simulates the depth perception ability of human binoculars. It consists of two cameras arranged side by side and can capture two slightly different perspective images of the same scene. Before actually controlling the flexible endoscope manipulation robot system, it is necessary to perform hand-eye calibration on the binocular camera 500. Hand-eye calibration can be understood as a process for determining the relationship between the coordinate system of the binocular camera 500 and the robot coordinate system. In other words, through hand-eye calibration, the conversion relationship between the coordinate system of the binocular camera 500 and the robot coordinate system can be obtained. In this way, for the image information collected during the actual operation of the binocular camera 500, the coordinate values of the points in the image information in the robot coordinate system can be obtained.

[0099] In some other embodiments of the present application, point cloud data of obstacles in the surgical environment can also be obtained through a lidar scanning system, a depth camera, etc.

[0100] Based on this, in the robotic arm trajectory planning method of the embodiments of the present application, obtaining the point cloud data of obstacles in the surgical environment and fitting the pose of the obstacle bounding box in the robot coordinate system includes:

[0101] Collect the image information of the obstacles in the surgical environment, and obtain the point cloud data of the obstacles in the surgical environment through image matching and three-dimensional reconstruction techniques;

[0102] Based on the obtained point cloud data of the obstacles in the surgical environment, use the bounding box algorithm to fit the pose of the obstacle bounding box in the robot coordinate system. The bounding box algorithm includes at least one of an axis-aligned bounding box and an oriented bounding box.

[0103] With the above settings, by obtaining the point cloud data of the obstacles in the surgical environment and fitting the pose of the obstacle bounding box in the robot coordinate system, the obstacles in the surgical environment can be accurately identified and located. This step provides accurate environmental information for subsequent trajectory planning, enabling the robot to effectively avoid obstacles when planning the motion trajectory and preventing damage to the flexible endoscope due to improper operation.

[0104] S202: Obtain the poses of the tool center points at the end of the operating arm and the tool center points at the end of the conveying arm in the robot coordinate system, and fit the robot bounding box in combination with the robot structure parameters.

[0105] Specifically, through the forward kinematics model of the robot, according to the angle values of each joint of the robot, using the kinematic transformation formula, the poses of the tool center points at the end of the operating arm and the tool center points at the end of the conveying arm in the robot coordinate system can be calculated to determine their spatial positions.

[0106] Given the structural parameters such as the dimensions and shapes of the various components of the robot, and the spatial positions of the tool center points at the end of the operating arm and the tool center points at the end of the conveying arm obtained through the forward kinematics model of the robot, with the tool center points at the end of the operating arm and the tool center points at the end of the conveying arm as references, in combination with the relative position relationships between the various components and the tool center points at the end of the operating arm and the tool center points at the end of the conveying arm, determine the positions and postures of the robot body, the conveying device 103, and the operating device 104 in the robot coordinate system. Then, for the robot body, the conveying device 103, and the operating device 104, use a suitable bounding box algorithm, such as the axis-aligned bounding box algorithm, to calculate the smallest bounding box that can enclose the robot body, the conveying device 103, and the operating device 104, which is used to evaluate the spatial occupancy of the robot during movement and prevent collisions with obstacles.

[0107] The robot bounding box includes the robot body bounding box , where:

[0108] —— The rotation matrix of the robot body bounding box rotating based on the Z-axis Angle;

[0109] —— The initial position point set of the robot body bounding box based on the robot coordinate system;

[0110] —— The zero position offset of the robot body bounding box, which determines the spatial position of the robot body bounding box in the robot coordinate system.

[0111] Conveyor device bounding box , where:

[0112] —— The rotation matrix of the conveyor device bounding box rotating based on the Z-axis Angle;

[0113] —— The initial position point set of the conveyor device bounding box based on the robot coordinate system;

[0114] —— The zero position offset of the conveyor device bounding box, which determines the spatial position of the conveyor device bounding box in the robot coordinate system.

[0115] And the operating device bounding box , where:

[0116] —— The rotation matrix of the operating device bounding box rotating based on the Z-axis Angle;

[0117] —— The initial position point set of the operating device bounding box based on the robot coordinate system;

[0118] —— The zero position offset of the operating device bounding box, which determines the spatial position of the operating device bounding box in the robot coordinate system.

[0119] With the above settings, the pose of the tool center point at the end of the manipulator and the tool center point at the end of the conveyor arm in the robot coordinate system is determined through the forward kinematics of the robot, and the robot bounding box is fitted in combination with the robot structure parameters, accurately describing the motion range and posture of the robot, providing the kinematic information of the robot itself for trajectory planning, enabling the robot to better adapt to different surgical environments and operation requirements, and improving the flexibility and adaptability of the system.

[0120] S203: The length of the trajectory to be fitted is obtained based on the current endoscope conveying length, and the obstacle avoidance space is obtained based on the position of the obstacle bounding box in the robot coordinate system. The center point of the tool at the end of the operating arm is used as the starting point, and the center point of the tool at the end of the conveying arm is used as the ending point. The length of the trajectory to be fitted and the obstacle avoidance space are combined to generate a motion trajectory curve of the operating arm.

[0121] In the flexible endoscope operating robot system, the conveying device 103 conveys the flexible endoscope 1 through the natural cavity into the human body. During the delivery process, the conveying arm 101 will keep the conveying position stopped, and the conveying device 103 at the end of the conveying arm 101 conveys the flexible endoscope 1. The end of the operating arm 102 supports and fixes the flexible endoscope 1, operates the bending of the flexible endoscope 1, and the operating arm 102 moves with the flexible endoscope 1 in the direction close to the conveying arm 101. In the embodiment of the present application, the robot arm trajectory planning is the operating arm trajectory planning. Therefore, the motion trajectory curve of the operating arm uses the center point of the tool at the end of the operating arm as the starting point of the trajectory planning. , with the center point of the tool at the end of the conveyor arm as the end point .

[0122] Among them, the obstacle avoidance space is obtained based on the position of the obstacle bounding box in the robot coordinate system. Specifically, in the mathematical model, by setting appropriate constraints, such as setting a certain safety distance around the obstacle bounding box to form an obstacle avoidance space, it is ensured that the planned operating arm trajectory will not enter the dangerous area, and the collision between the operating arm and the obstacle is avoided as much as possible.

[0123] Among them, the length of the trajectory to be fitted is obtained based on the current endoscope delivery length. The soft endoscope manipulation robot system of the embodiment of the present application also includes a delivery length sensor installed on the delivery arm 101. In the robot arm trajectory planning method of the embodiment of the present application, the length of the trajectory to be fitted is obtained based on the current endoscope delivery length, including:

[0124] The current endoscope delivery length is obtained through a delivery length sensor;

[0125] The length of the trajectory to be fitted is obtained by combining the current endoscope delivery length and the endoscope length parameter.

[0126] The current endoscope delivery length is the length of the flexible endoscope 1 extending from the delivery arm 101 , and the difference between the endoscope length parameter and the current endoscope delivery length is the length of the trajectory to be fitted.

[0127] Based on the determined starting point and ending point of trajectory planning, the length of the trajectory to be fitted and the obstacle avoidance space, a suitable mathematical model, such as a spline curve model, is adopted. By adjusting the control points and parameters of the curve, the curve can avoid the obstacle avoidance space and maintain smoothness while meeting the starting point and ending point conditions.

[0128] Furthermore, in the robotic arm trajectory planning method according to the embodiments of the present application, taking the center point of the tool at the end of the operating arm as the starting point and the center point of the tool at the end of the conveying arm as the end point, and combining the length of the trajectory to be fitted and the obstacle avoidance space, a motion trajectory curve of the operating arm is generated, including:

[0129] The motion trajectory curve is described by a B-spline curve, and the B-spline curve is solved through an optimization model. The objective function of the optimization model is:

[0130]

[0131] Wherein:

[0132] —Control points affecting the current curve value;

[0133] —Knot vector;

[0134] —Integral of the square of the second derivative of the curve;

[0135] —Weight coefficient;

[0136] —B-spline curve;

[0137] —Knot;

[0138] —Total length of the endoscope;

[0139] —Current endoscope conveying length;

[0140] The constraint conditions of the optimization model include:

[0141]

[0142]

[0143]

[0144] Wherein:

[0145] —Kth B-spline basis function;

[0146] —Starting point;

[0147] —End point;

[0148] —Obstacle bounding box;

[0149] — Avoidance safety distance.

[0150] Specifically, the expression of the B-spline curve is as follows:

[0151]

[0152] is the control point affecting the current curve value, is the k-th order B-spline basis function, which can be calculated by the De Boor-Cox recurrence formula:

[0153]

[0154] Knot vector , and satisfies .

[0155] Where:

[0156] First, the starting point of the trajectory planning needs to satisfy , that is, the starting point constraint condition is expressed as: .

[0157] Second, the ending point of the trajectory planning needs to satisfy , that is, the ending point constraint condition is expressed as: .

[0158] Third, the arc length L calculation formula of the B-spline curve is:

[0159]

[0160] is the first derivative of the curve, , and are the starting and ending parameters within the domain of the curve, , , given the current endoscope delivery length , the remaining arc length is calculated according to the total endoscope length to calculate the B-spline arc length, then the arc length constraint condition is expressed as: .

[0161] Fourth, the curve needs to avoid the obstacle space area position , so the obstacle avoidance space is expressed as: , is the avoidance safety distance.

[0162] Fifth, to ensure the smoothness of the curve, the second derivative of the curve can be constrained. Define the second derivative of the curve as , smoothness can be ensured by minimizing the integral of the square of the second derivative of the curve, that is, a part of the objective function can be set as: .

[0163] Based on the above constraints, the optimization model is constructed as above. Among them, is the weight coefficient, which is used to balance the relative importance of the curve length constraint and the smoothness constraint. By calculating the optimal control points and knot vectors through this optimization model, the motion trajectory curve can be obtained, which significantly improves the reliability and safety of the trajectory planning.

[0164] S204: Extract the trajectory points from the motion trajectory curve and plan the posture of the manipulator at each trajectory point.

[0165] On the basis of the fitted bounding box of the manipulator, the motion trajectory curve is equally divided by unit length (1 mm), and the equally divided points are the trajectory points. Analyze the influence of the posture of the manipulator 102 at this trajectory point on the spatial position of the bounding box.

[0166] By changing the angles of each joint of the manipulator 102, adjust the posture of the manipulator 102 at each trajectory point, and then make the spatial position of the bounding box change accordingly to meet the obstacle avoidance and motion requirements of the manipulator 102 at this trajectory point. For example, when the manipulator 102 approaches an obstacle, by adjusting the joint angles, make the manipulator 102 tilt away from the obstacle, so as to change the direction and position of the bounding box and avoid collision with the obstacle.

[0167] Based on this, in the manipulator trajectory planning method of this application embodiment, extracting the trajectory points from the motion trajectory curve includes:

[0168] Equally divide the motion trajectory curve by unit length, and the equally divided points are the trajectory points.

[0169] S205: Verify whether the trajectory points of the manipulator are reachable. If the trajectory points of the manipulator are not reachable, adjust the posture of the manipulator at the corresponding trajectory points; if the trajectory points of the manipulator are reachable, the trajectory points of the manipulator are matched by the current endoscope delivery length, and the trajectory points of the manipulator are described by the pose of the manipulator.

[0170] Among them, in the manipulator trajectory planning method of this application embodiment, verifying whether the trajectory points of the manipulator are reachable includes:

[0171] Verify whether the first constraint condition and the second constraint condition are simultaneously satisfied. If the first constraint condition and the second constraint condition are simultaneously satisfied, the trajectory points of the manipulator are reachable; if the first constraint condition and the second constraint condition are not simultaneously satisfied, the trajectory points of the manipulator are not reachable;

[0172] The first constraint is that when moving from the previous adjacent trajectory point to the corresponding trajectory point, the rotation angles of the joints of the manipulator do not exceed the rotation angle range;

[0173] The second constraint is that at the corresponding trajectory point, there is no interference between the robot bounding box and the obstacle bounding box.

[0174] Regarding the rotation angle limitations of the joints of the robotic arm, considering the physical limitations and kinematic constraints of the robotic arm joints, each joint has its own rotation angle range. During the verification process, calculate the change in the rotation angles of the joints of the manipulator when moving from the previous adjacent trajectory point to the corresponding trajectory point. If the rotation angles of the joints of the manipulator do not exceed the rotation angle range, it is determined that the first constraint is satisfied; if the rotation angles of the joints of the manipulator exceed the rotation angle range, it is determined that the first constraint is not satisfied and this trajectory point is unreachable.

[0175] Regarding the spatial reachability of the robotic arm, combined with the fitted robot bounding box, check whether there is interference between the robot bounding box and the obstacle bounding box at the current trajectory point. If there is no interference between the robot bounding box and the obstacle bounding box, it is determined that the second constraint is satisfied and this trajectory point is reachable. If there is interference between the robot bounding box and the obstacle bounding box, it is determined that the second constraint is not satisfied and this trajectory point is unreachable. It is necessary to adjust the rotation posture of the robotic arm, adjust the rotation matrix of the spatial bounding box of the robotic arm, change the spatial position of the bounding box of the robotic arm, and perform obstacle avoidance pose adjustment of the robotic arm;

[0176] In the flexible endoscope manipulation robot system, the trajectory points of the manipulator are matched by the current endoscope delivery length. That is, during the surgical operation, the delivery length sensor real-time feedbacks the current endoscope delivery length, and searches for the corresponding trajectory points in the verified and effective motion trajectory curve according to the current endoscope delivery length. Then, by controlling the movement of the joints of the robot, the operating device 104 is accurately moved to this trajectory point to ensure that the flexible endoscope is safely and accurately delivered along the planned trajectory. During the movement process, continuously monitor the current endoscope delivery length and the position of the operating device 104, and real-time adjust the motion parameters of each joint to ensure the accuracy and stability of the movement.

[0177] The trajectory points of the manipulator are described by the pose of the manipulator, that is, the pose of each trajectory point is determined, and the pose includes the spatial position and the attitude.

[0178] In some possible implementation manners, the robotic arm trajectory planning method of the embodiment of the present application further includes:

[0179] When the obstacle bounding box enters the working space of the manipulator, and the pose of the obstacle bounding box changes, and the obstacle bounding box is on the motion trajectory curve, a new motion trajectory curve is regenerated, and the manipulator switches to the action of the new motion trajectory curve.

[0180] In this way, the manipulator trajectory planning method of the embodiment of the present application can respond to environmental changes in real time. When the obstacle bounding box enters the working space of the manipulator, and the pose of the obstacle bounding box changes, and the obstacle bounding box is on the motion trajectory curve, a new motion trajectory curve is regenerated, and the manipulator switches to the action of the new motion trajectory curve, ensuring that the robot can accurately avoid obstacles, better adapt to the complex and changeable surgical environment, and guarantee the safety and reliability of the robot's movement.

[0181] Based on the manipulator trajectory planning method provided by the above method embodiment, the embodiment of the present application also provides a manipulator trajectory planning device. The manipulator trajectory planning device will be described below with reference to the drawings. Since the principle of solving problems by the device in the embodiments of the present disclosure is similar to the above manipulator trajectory planning method of the embodiments of the present application, the implementation of the device can refer to the implementation of the method, and the repeated parts will not be described again.

[0182] Please refer to Figure 3 , Figure 3 which is a structural schematic diagram of a specific embodiment of the manipulator trajectory planning device provided by the present application.

[0183] The embodiment of the present application also provides a manipulator trajectory planning device 600, which is applied to a flexible endoscope control robot system. The flexible endoscope control robot system includes a delivery arm 101 and a manipulator arm 102; the manipulator trajectory planning device 600 includes:

[0184] A first fitting unit 601, configured to obtain the point cloud data of the obstacles in the surgical environment and fit the pose of the obstacle bounding box in the robot coordinate system;

[0185] A second fitting unit 062, configured to determine the poses of the tool center points at the end of the manipulator arm and the tool center points at the end of the delivery arm in the robot coordinate system through forward kinematics of the robot, and fit the robot bounding box in combination with the robot structure parameters;

[0186] A generating unit 603, configured to obtain the length of the trajectory to be fitted based on the current endoscope delivery length, obtain the obstacle avoidance space based on the pose of the obstacle bounding box in the robot coordinate system, use the tool center point at the end of the manipulator arm as the starting point, use the tool center point at the end of the delivery arm as the ending point, and generate the motion trajectory curve of the manipulator arm in combination with the length of the trajectory to be fitted and the obstacle avoidance space;

[0187] A planning unit 604, configured to extract trajectory points from the motion trajectory curve and plan the poses of the manipulator arm at each trajectory point;

[0188] A verification unit 605 is configured to verify whether the trajectory points of the robotic arm are reachable. In the case where the trajectory points of the robotic arm are unreachable, the posture of the robotic arm at the corresponding trajectory points is adjusted; in the case where the trajectory points of the robotic arm are reachable, the trajectory points of the robotic arm are matched with the current endoscope delivery length, and the trajectory points of the robotic arm are described by the pose of the robotic arm.

[0189] In some possible implementation manners, the first fitting unit 601 includes:

[0190] A first acquisition subunit is configured to collect image information of obstacles in the surgical environment, and obtain point cloud data of the obstacles in the surgical environment through image matching and three-dimensional reconstruction technologies;

[0191] A first fitting subunit is configured to, based on the obtained point cloud data of the obstacles in the surgical environment, fit the pose of the obstacle bounding box in the robot coordinate system by using a bounding box algorithm, where the bounding box algorithm includes at least one of an axis-aligned bounding box and an oriented bounding box.

[0192] It should be noted that the specific implementation of each unit in this embodiment can refer to the relevant descriptions in the above method embodiment. The division of units in the embodiments of the present application is illustrative, and is only a logical function division. In actual implementation, there may be other division methods. The functional units in the embodiments of the present application can be integrated into one processing unit, or each unit can exist physically alone, or two or more units can be integrated into one unit. The above integrated unit can be implemented in the form of hardware or in the form of a software functional unit.

[0193] The embodiments of the present application further provide a computing device, which can be a cloud computing device or an edge computing device, and includes: a processor, a memory, and a system bus;

[0194] The processor and the memory are connected through the system bus;

[0195] The memory is used to store one or more programs, where one or more programs include instructions, and when the instructions are executed by the processor, the processor is caused to execute the above robotic arm trajectory planning method.

[0196] The present application further provides a computer-readable storage medium, where computer-readable instructions are stored in the computer-readable storage medium. When the computer-readable instructions run on a computing device, the computing device is caused to execute the above robotic arm trajectory planning method.

[0197] In the context of the present application, a computer-readable storage medium, also known as a machine-readable medium, can be a tangible medium that can contain or store a program for use by or in connection with an instruction execution system, apparatus, or device. The machine-readable medium can be a machine-readable signal medium or a machine-readable storage medium. The machine-readable medium can include, but is not limited to, electronic, magnetic, optical, electromagnetic, infrared, or semiconductor systems, devices, or apparatuses, or any suitable combination of the foregoing. More specific examples of the machine-readable storage medium would include electrical connections based on one or more wires, portable computer disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fibers, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination of the foregoing.

[0198] It should be noted that the computer-readable medium described above in the present application can be a computer-readable signal medium, a computer-readable storage medium, or any combination of the two. The computer-readable storage medium can be, for example, but not limited to, electrical, magnetic, optical, electromagnetic, infrared, or semiconductor systems, devices, or components, or any combination of the above. More specific examples of the computer-readable storage medium can include, but are not limited to: electrical connections with one or more wires, portable computer disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fibers, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination of the above. In the present application, the computer-readable storage medium can be any tangible medium that contains or stores a program that can be used by or in connection with an instruction execution system, apparatus, or device. And in the present application, the computer-readable signal medium can include a data signal propagated in a baseband or as part of a carrier wave, in which the computer-readable program code is carried. Such a propagated data signal can take various forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination of the above. The computer-readable signal medium can also be any computer-readable medium other than the computer-readable storage medium, and the computer-readable signal medium can send, propagate, or transmit a program for use by or in connection with an instruction execution system, apparatus, or device. The program code contained on the computer-readable medium can be transmitted using any appropriate medium, including but not limited to: wires, optical cables, RF (radio frequency), etc., or any suitable combination of the above.

[0199] Although the subject matter has been described in language specific to structural features and / or methodological logical acts, it should be understood that the subject matter defined in the appended claims is not necessarily limited to the specific features or acts described above. On the contrary, the specific features and acts described above are merely example forms for implementing the claims.

[0200] Although several specific implementation details are included in the above discussion, these should not be construed as limiting the scope of the present application. Certain features described in the context of separate embodiments can also be implemented in combination in a single embodiment. Conversely, the various features described in the context of a single embodiment can also be implemented separately or in any suitable sub-combination in multiple embodiments.

[0201] The above description is only a preferred embodiment of the present application and an explanation of the applied technical principles. Those skilled in the art should understand that the scope of disclosure involved in the present application is not limited to the technical solutions formed by the specific combination of the above-mentioned technical features, and should also cover other technical solutions formed by any combination of the above-mentioned technical features or their equivalent features without departing from the above-mentioned disclosure concept. For example, the technical solutions formed by mutually replacing the above-mentioned features with the technical features (but not limited to) having similar functions disclosed in the present application.

Claims

1. A manipulator trajectory planning method, characterized in that, Applied to a flexible endoscope manipulation robot system, the flexible endoscope manipulation robot system includes a delivery arm and an operating arm, and the method includes: Obtaining point cloud data of obstacles in the surgical environment and fitting the pose of the obstacle bounding box in the robot coordinate system; Obtaining the poses of the tool center points at the end of the operating arm and the tool center points at the end of the delivery arm in the robot coordinate system, and fitting the robot bounding box in combination with the robot structure parameters; Based on the current endoscope delivery length, obtaining the length of the trajectory to be fitted, and based on the pose of the obstacle bounding box in the robot coordinate system, obtaining the obstacle avoidance space. Taking the tool center point at the end of the operating arm as the starting point and the tool center point at the end of the delivery arm as the ending point, and combining the length of the trajectory to be fitted and the obstacle avoidance space, generating the motion trajectory curve of the operating arm; Extracting trajectory points from the motion trajectory curve and planning the poses of the operating arm at each of the trajectory points; Verifying whether the trajectory points of the operating arm are reachable. In the case where the trajectory points of the operating arm are not reachable, adjusting the pose of the operating arm at the corresponding trajectory points; in the case where the trajectory points of the operating arm are reachable, the trajectory points of the operating arm are matched by the current endoscope delivery length, and the trajectory points of the operating arm are described by the pose of the operating arm.

2. The method according to claim 1, characterized in that The obtaining point cloud data of obstacles in the surgical environment and fitting the pose of the obstacle bounding box in the robot coordinate system includes: Collecting image information of obstacles in the surgical environment and obtaining the point cloud data of obstacles in the surgical environment through image matching and three-dimensional reconstruction techniques; Based on the obtained point cloud data of obstacles in the surgical environment, using a bounding box algorithm to fit the pose of the obstacle bounding box in the robot coordinate system, and the bounding box algorithm includes at least one of an axis-aligned bounding box and an oriented bounding box.

3. The method according to claim 1, characterized in that, The flexible endoscope manipulation robot system further includes a delivery length sensor installed on the delivery arm, and the obtaining the length of the trajectory to be fitted based on the current endoscope delivery length includes: Obtaining the current endoscope delivery length through the delivery length sensor; Combining the current endoscope delivery length and the endoscope length parameter to obtain the length of the trajectory to be fitted.

4. The method according to claim 1, characterized in that Taking the tool center point at the end of the operating arm as the starting point and the tool center point at the end of the delivery arm as the ending point, and combining the length of the trajectory to be fitted and the obstacle avoidance space, generating the motion trajectory curve of the operating arm includes: The motion trajectory curve is described by a B-spline curve, and the B-spline curve is solved through an optimization model, and the objective function of the optimization model is: ； Where: — Control points affecting the current curve value; — Knot vector; — Square integral of the second derivative of the curve; — Weight coefficient; —B-spline curve; — node; — Total length of the endoscope; — Current endoscope delivery length; The constraint conditions of the optimization model include: ； ； ； Where: — B-spline basis function of order k; — Starting point; — termination point; — Obstacle bounding box; — Obstacle avoidance safety distance.

5. The method according to claim 1, wherein The extracting trajectory points from the motion trajectory curve includes: Dividing the motion trajectory curve into equal parts by unit length, and the equal division points are the trajectory points.

6. The method according to claim 1, wherein The verifying whether the trajectory points of the operating arm are reachable includes: Verify whether the first constraint condition and the second constraint condition are both satisfied. When both the first constraint condition and the second constraint condition are satisfied, the trajectory point of the manipulator is reachable. When the first constraint condition and the second constraint condition are not both satisfied, the trajectory point of the manipulator is unreachable; The first constraint condition is that when moving from the adjacent previous trajectory point to the trajectory point, the rotation angles of the joints of the manipulator do not exceed the rotation angle range; The second constraint condition is that at the trajectory point, the robot bounding box and the obstacle bounding box do not interfere with each other.

7. The method according to any one of claims 1-6, characterized in that The method further includes: When the obstacle bounding box enters the working space of the manipulator, and the pose of the obstacle bounding box changes, and the obstacle bounding box is on the motion trajectory curve, regenerate the motion trajectory curve, and the manipulator switches to the new motion trajectory curve for action.

8. A robotic arm trajectory planning device, characterized in that, Applied to a flexible endoscope manipulation robot system, the flexible endoscope manipulation robot system includes a delivery arm and a manipulator; the device includes: A first fitting unit for obtaining the point cloud data of the obstacles in the surgical environment and fitting the pose of the obstacle bounding box in the robot coordinate system; A second fitting unit for obtaining the poses of the tool center point at the end of the manipulator and the tool center point at the end of the delivery arm in the robot coordinate system, and fitting the robot bounding box in combination with the robot structure parameters; A generating unit for obtaining the length of the trajectory to be fitted based on the current endoscope delivery length, obtaining the obstacle avoidance space based on the pose of the obstacle bounding box in the robot coordinate system, taking the tool center point at the end of the manipulator as the starting point, taking the tool center point at the end of the delivery arm as the ending point, and generating the motion trajectory curve of the manipulator in combination with the length of the trajectory to be fitted and the obstacle avoidance space; A planning unit for extracting trajectory points from the motion trajectory curve and planning the poses of the manipulator at each of the trajectory points; A verification unit for verifying whether the trajectory points of the manipulator are reachable. When the trajectory points of the manipulator are unreachable, adjust the pose of the manipulator at the corresponding trajectory points; when the trajectory points of the manipulator are reachable, match the trajectory points of the manipulator with the current endoscope delivery length, and the trajectory points of the manipulator are described by the pose of the manipulator.

9. The device according to claim 8, characterized in that, The first fitting unit includes: A first obtaining subunit for collecting the image information of the obstacles in the surgical environment and obtaining the point cloud data of the obstacles in the surgical environment through image matching and three-dimensional reconstruction techniques; A first fitting subunit for fitting the pose of the obstacle bounding box in the robot coordinate system based on the obtained point cloud data of the obstacles in the surgical environment by using a bounding box algorithm, and the bounding box algorithm includes at least one of an axis-aligned bounding box and an oriented bounding box.

10. A computing device, characterized in that, The computing device includes a processor and a memory, and instructions are stored in the memory. The processor executes the instructions to enable the processor to execute the robotic arm trajectory planning method according to any one of claims 1 to 7.

11. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores computer-readable instructions that, when run on a computing device, cause the computing device to execute the robotic arm trajectory planning method according to any one of claims 1 to 7.

Images