CN120053077B

CN120053077B - Endoscopic Control Method for Neurosurgical Robots Based on Hierarchical Quadratic Programming Framework

Info

Publication number: CN120053077B
Application number: CN202510199601.8A
Authority: CN
Inventors: 李亚; 裴葆青; 武雪晴
Original assignee: Beihang University
Current assignee: Beihang University
Priority date: 2025-02-24
Filing date: 2025-02-24
Publication date: 2025-11-21
Anticipated expiration: 2045-02-24
Also published as: CN120053077A

Abstract

The technical field of endoscopic control of the external operation robot, in particular to an endoscopic control method of the external operation robot based on a hierarchical quadratic programming framework, which comprises the steps of determining the running mode of the current external operation robot endoscope, wherein the running mode comprises a pivot motion mode, a path navigation mode and a dynamic tracking mode; according to the operation mode, a corresponding mechanical arm motion constraint model is determined based on a hierarchical quadratic programming method, the mechanical arm motion constraint model is solved based on output information of the position sensor and the force sensor to obtain the control speed of the robot joint, the robot joint is controlled to move based on the control speed, and finally the control of the endoscope of the external operation robot is completed.

Description

Endoscopic control method of external operation robot based on hierarchical quadratic programming framework

Technical Field

The invention relates to the technical field of endoscopic control of a surgical robot, in particular to a surgical robot endoscopic control method based on a hierarchical quadratic programming framework.

Background

The neuroendoscopic surgery is an effective method for treating neurosurgical diseases such as cerebral hemorrhage. Endoscopes are widely used as important surgical instruments in minimally invasive surgery. The traditional endoscope operation mainly depends on manual control of doctors, which not only increases the operation burden of the doctors, but also can cause the problems of unstable images, limited visual field and the like due to human factors.

With the continuous development of surgical robot technology, the prior art adopts a mechanical arm to realize endoscope motion control, and particularly, a mature multi-arm type active surgical robot is developed in the field of laparoscopic surgery, and the mode is explored in neurosurgery skull base clinical operation, but the end effector is large in size and suitable for the operation mode with large surgical areas such as abdominal cavity, and the application in the field of neurosurgery is still difficult. So the neurosurgery is mostly realized by adopting a single-arm mode to realize the motion control of the endoscope at present.

In the prior art, the control of the movement of the mechanical arm to drive the endoscope has obvious defects in the extraterrestrial operation. First, this method cannot identify environmental factors, resulting in low surgical safety. In the process that the endoscope enters the focus area from the head bone point, the collision risk of cerebral blood vessels and important brain tissues and the state of the mechanical arm are considered, but the factors cannot be synthesized in real time by controlling the movement of the mechanical arm through keys to drive the endoscope, so that the operation risk is increased. Secondly, the various movement modes (such as forward, backward, rotation, etc.) of the endoscope need to be manually switched at different stages, which increases the operation complexity of doctors. Therefore, the conventional control method is insufficient in safety and operation convenience.

Disclosure of Invention

In view of the above problems, the invention provides a method for controlling an endoscope of an external operation robot based on a hierarchical quadratic programming framework, which solves the technical problems of low endoscope control safety and high complexity in the prior art.

The invention provides a method for controlling an endoscope of a nerve external operation robot based on a layered quadratic programming framework, which comprises an endoscope body, a mechanical arm, a position sensor and a force sensor, wherein the mechanical arm is provided with a plurality of robot joints and is characterized by comprising the following steps:

Step S1, determining the running mode of the current external operation robot endoscope, wherein the running mode comprises a pivot motion mode, a path navigation mode and a dynamic tracking mode;

Step S2, determining a corresponding mechanical arm motion constraint model based on a hierarchical quadratic programming method according to the operation mode, wherein the method specifically comprises the following steps:

If the operation mode is a pivot motion mode, establishing a first constraint relation of a distance between a projection position of a pivot point on the endoscope body and a position of the pivot point and a speed of the robot joint as the mechanical arm motion constraint model;

If the operation mode is a path navigation mode, establishing a second constraint relation of pose difference between the mechanical arm and a target area, distance between an endoscope body and a key environment structure and speed of the robot joint as a mechanical arm motion constraint model;

if the operation mode is a dynamic tracking mode, establishing a third constraint relation of pose difference between the mechanical arm and the instrument to be tracked, contact force of the tail end of the mechanical arm and the speed of the robot joint as a mechanical arm motion constraint model;

s3, solving the mechanical arm motion constraint model based on the output information of the position sensor and the force sensor to obtain the control speed of the robot joint;

and S4, controlling the robot joint to move based on the control speed, and finally completing the endoscopic control of the surgical robot.

Preferably, in step S1:

In the pivotal movement mode, the endoscope body moves around a pivot point;

In the path navigation mode, the endoscope body reaches a target area through path navigation movement;

In the dynamic tracking mode, the endoscope body moves along with the instrument to be tracked.

Preferably, in step S2:

the first constraint relation is used for constraining the mechanical arm to move around the pivot point and constraining the upper speed limit of the robot joint;

the second constraint relation is used for constraining the movement speed of the mechanical arm between the current pose and the pose reaching the target area, preventing the mechanical arm from colliding with a key environment structure when moving, and constraining the upper speed limit of the robot joint;

the third constraint relation is used for constraining the relative motion between the mechanical arm and the instrument to be tracked, constraining the stress of the tail end of the mechanical arm and constraining the upper speed limit of the robot joint.

Preferably, in step S2, the expression of the first constraint relation is:

r_rcm＝-‖P_rcm-P_current‖

Where min represents the need to minimize the objective function, II ² represents the square of the calculated vector modulus, s.t. represents the satisfaction of the following constraints, Indicating the speed of the robot joint,For the maximum speed limit threshold, J _rcm represents the jacobian matrix of the RCM task,An estimate of the position vector between the pivot point and the projection point is represented, δP _rcm represents the change in position of the pivot point, δq represents the change in position of the robot joint, r _rcm is the residual of the RCM task, P _rcm represents the pivot point position, P _current represents the projection point position of the pivot point on the neuroendoscope axis, and II represents the modulus of the calculated vector.

Preferably, in step S2, the expression of the second constraint relation is:

r_trajPlanning＝log(T_focusT_act ^-1)

r_coll＝‖d_i‖

d_i＝p_a-p_c

Wherein K _t1,K_t2 is the weight coefficient of the path planning and collision avoidance task, K _r1,K_r2 is the residual ratio coefficient of the path planning and collision avoidance task, J _trajPlanning is the Jacobian matrix of the path planning task, delta T _act is the change of the current pose of the path planning task, r _trajPlanning is the residual of the path planning task, log (&) is the natural logarithm, T _focus is the pose reaching the target area, T _act ^-1 is the inverse matrix of the current pose T _act of the path planning task, J _coll is the Jacobian matrix of the collision avoidance task, d _i ^T is the transpose of d _i, delta d _i ^Td_i is the change of the product of d _i ^T and d _i, p _c is the position of the critical environment structure, p _a is the nearest point position of the endoscope from the critical environment structure, r _coll is the residual of the collision avoidance task

Preferably, in step S2, the expression of the third constraint relation is:

r_track＝log(T_desT_act ^-1)

Wherein, K _t3,K_t4 is the weight coefficient of the tracking task and the avoiding task, K _r3 is the residual error proportionality coefficient of the avoiding task, J _track is the Jacobian matrix of the tracking task, delta X _act is the change of the current pose of the tracking task, r _track is the residual error of the tracking task, X _act is the current pose of the tracking task, X _act ^-1 is the inverse matrix of X _act, X _des is the target pose of the tracking task, J _force is the Jacobian matrix of the avoiding task, alpha is the adjustment coefficient, and F _ernd is the tail end force of the mechanical arm.

Preferably, in the pivot motion mode, the position sensor of the endoscope of the surgical robot acquires the position P _rcm of a pivot point in real time, and the position P _current of the projection point is calculated according to the position of the pivot point and by a geometric relation;

in the path navigation mode, acquiring the current pose T _act and the nearest point position p _a of the endoscope from a key environment structure in real time by a position sensor of the endoscope of the external surgical robot;

And in the dynamic tracking mode, acquiring the tail end force F _end of the mechanical arm in real time by the force sensor.

Preferably, step S3 specifically includes:

And carrying out optimization solution on constraint conditions in the mechanical arm motion constraint model based on the output information of the position sensor and the force sensor, and obtaining the optimal real-time speed of the robot joint as the control speed of the robot joint.

Compared with the prior art, the invention has at least the following beneficial effects:

(1) According to the invention, the operation mode of the endoscope is divided into three modes of pivot motion, path navigation and dynamic tracking, and a corresponding mechanical arm motion constraint model is established for each mode, so that the robot endoscope is accurately controlled. By constructing the constraint relation of the distance between the projection position of the pivot point and the actual position, the accurate rotation of the endoscope around the far end point is ensured, and the risk that the mechanical arm joint reaches the limit range is effectively avoided.

(2) According to the intelligent avoidance method, the intelligent avoidance of the key environment structure is realized by establishing the constraint relation between the pose difference between the mechanical arm and the target area and the distance between the endoscope body and the key environment structure. The control method based on layered quadratic programming can automatically adjust the movement track of the mechanical arm, reduce the risk of damaging the key environment structure in the running process of the endoscope to the greatest extent, and reduce the complexity of operation.

(3) According to the invention, the pose difference and the tail end contact force between the mechanical arm and the instrument to be tracked are taken into the constraint model, so that the tracking of the endoscope to the surgical instrument is realized. Through the output information of real-time processing position sensor and force sensor, the system can keep surgical instrument in the field of vision center all the time, avoids taking place unexpected contact with other instruments simultaneously, has promoted the accuracy and the smoothness of scope operation.

Drawings

The drawings are only for purposes of illustrating particular embodiments and are not to be construed as limiting the invention.

Fig. 1 is a flowchart of a method for controlling an endoscope of an extraterrestrial surgical robot based on a hierarchical quadratic programming framework.

Fig. 2 is a schematic view of the present invention providing movement of the endoscope about a pivot point.

Fig. 3 is a schematic diagram of a hierarchical quadratic programming control framework provided by the invention.

Detailed Description

In order that the above-recited objects, features and advantages of the present invention will be more clearly understood, a more particular description of the invention will be rendered by reference to the appended drawings and appended detailed description. It should be noted that, without conflict, the embodiments of the present invention and features in the embodiments may be combined with each other. In addition, the invention may be practiced otherwise than as specifically described and thus the scope of the invention is not limited by the specific embodiments disclosed herein.

For the surgical robot endoscope, the motion of the robot endoscope is divided into three modes, namely pivot motion, path navigation and dynamic tracking. In the moving process of the robot endoscope, the three modes are different in operation, so that corresponding control tasks are inconsistent. In an actual operation, the endoscope movement cannot touch important key environment structures, control constraint corresponds to mechanical arm control, and meanwhile joint limitation, singular position limitation and the like exist in a mechanical arm system. For this reason, the present invention has motion control and motion constraint in the control of each mode of the mechanical arm. The unified scheduling of a plurality of targets in a plurality of modes is realized through the layered controller, so that different moving task targets can be considered simultaneously, thereby realizing the safety control of endoscope operation and the stable transition between tasks, realizing the switching of endoscope movement without manually switching keys, simultaneously considering environmental factors in each movement, and improving the safety of the endoscope movement.

In order to illustrate the effectiveness of the method provided by the invention, the technical scheme of the invention is described in detail by a specific embodiment, as shown in fig. 1, a method for controlling an endoscope of an external operation robot based on a hierarchical quadratic programming framework is disclosed, the endoscope of the external operation robot comprises an endoscope body, a mechanical arm, a position sensor, a force sensor and the like, the mechanical arm is provided with a plurality of robot joints, and the specific implementation steps are as follows:

For the surgical robot endoscope, the motion of the robot endoscope is divided into three modes, namely a pivot motion mode, a path navigation mode and a dynamic tracking mode. Each mode has different control tasks for which constraints on the movement of the robot arm need to be determined.

In this step, the control tasks of the three modes are determined by a pre-programming. For the pivoting motion mode, the focus is on achieving rotational motion of the endoscope body about the pivot point. For the path navigation mode, path optimization needs to be completed, so that the path is ensured to advance along a preset track, and collision with a key environment structure is avoided. For the dynamic tracking mode, it is desirable to complete the tracking motion of the endoscope body with respect to other surgical instruments and reduce contact forces with other surgical instruments.

Step S2, determining a corresponding mechanical arm motion constraint model based on hierarchical quadratic programming according to the operation mode, wherein the method specifically comprises the following steps:

If the operation mode is a pivot motion mode, establishing a constraint relation of a distance between a projection position of a pivot point on the endoscope body and a position of the pivot point and a speed of the robot joint as the mechanical arm motion constraint model;

If the operation mode is a path navigation mode, establishing a constraint relation of pose difference between the mechanical arm and a target area, distance between an endoscope body and a key environment structure and speed of the robot joint as a mechanical arm motion constraint model;

if the operation mode is a dynamic tracking mode, establishing a constraint relation of pose difference between the mechanical arm and the instrument to be tracked, contact force of the tail end of the mechanical arm and the speed of the robot joint as a mechanical arm motion constraint model;

(1) Pivotal movement pattern

As shown in fig. 2, for the pivotal movement mode, the endoscope is used for probing operations, where a kinematic model is required to effect movement of the robotic arm about a pivot point, where the present invention determines the pivot point as a Remote Center of Motion (RCM) and the above movement task as an RCM task.

To ensure that the neuroendoscope rotates about the pivot point, the present invention creates an optimization problem that minimizes the distance of pivot point P _rcm∈R_3*1 from pivot point P _current∈R_3*1 to pivot point P _rcm∈R_3*1 on the neuroendoscope axis. Where R _3*1 represents the three-dimensional column vector space.

The jacobian matrix J _rcm∈R_1*6 that determines the relevant robot joint velocity and pivot point location for the RCM task, where R _1*6 represents a 6-dimensional row vector space, expressed as:

wherein J _rcm represents the Jacobian matrix of the RCM task, An estimated amount of the position vector between the pivot point and the projection point is represented, δp _rcm represents a change in position of the pivot point, and δq represents a change in position of the robot joint.

The residual of the RCM task for the pivot motion mode is used to describe the distance between the current projection point and the pivot point, expressed as:

r_rcm＝-‖p_e‖＝-‖P_rcm-P_current‖

Where r _rcm is the residual of the RCM task, p _e represents the position vector between the pivot point and the projection point, and II represents the modulus of the position vector.

In the process of the neuroendoscopic movement in the pivot movement mode, the self constraint of the mechanical arm is needed to be considered, the speed of the robot joint is limited, and the expression is as follows:

according to the above formula, the objective function for obtaining the pivot motion pattern is:

Where min represents the need to minimize the objective function, II ² represents the square of the calculated vector modulus, s.t. represents the satisfaction of the following constraints, Indicating the speed of the robot joint,A maximum speed limit threshold.

In some embodiments, the position of the pivot point P _rcm can be obtained in real time by the position sensor of the surgical robot endoscope, and the position of the pivot point projection point P _current on the neuroendoscope axis can be calculated from the geometric relationship according to the position of the pivot point.

(2) Path navigation mode

For the path navigation mode, the endoscope needs to pass through the path planning to reach the target region. The goal of the path planning task is the pose T _focus of the robotic joints of the robotic arm from the current pose T _act of the path planning task to the target area of the path planning task.

The pose comprises a position and a space orientation, and is used for completely describing the state of the robot joint of the mechanical arm in the three-dimensional space, and when the endoscope reaches the target area, the accuracy and the safety of the movement of the endoscope can be ensured only by considering whether the reached space position point is accurate and the orientation of the endoscope is proper.

The jacobian matrix expression of the current pose relative to the change relation of the joint angle is as follows:

where J _trajPlanning represents the jacobian matrix of the path planning task and δt _act represents the change in current pose.

The residual error of the path planning task is used for describing the deviation between the current pose and the target pose, and the expression is:

r_trajPlanning＝log(T_focusT_act ^-1)

Where e _trajPlanning represents the residual of the path planning task, log (·) represents the natural logarithm, T _focus represents the pose that reaches the target region, and T _act ^-1 represents the inverse of the current pose of the path planning task, T _act.

In the movement process of the endoscope reaching the target area, whether a mark touches a key environment structure needs to be judged, so that the path safety is ensured, the position of the key environment structure is p _c, the nearest point of the endoscope from the key environment structure is p _a, the vector defining the key environment structure and the nearest point is d _i＝p_a-p_c, and the expression of the Jacobian matrix representing the distance change relation between the endoscope and the nearest point of the key environment structure is as follows:

Where J _coll represents the Jacobian matrix of the collision avoidance task, II represents the modulus of the calculated vector, d _i ^T represents the transpose of d _i, δd _i ^Td_i represents the amount of change in the product of d _i ^T and d _i.

The residual error r _coll of the collision avoidance task is used for describing the distance between the endoscope and the key environment structure, and the expression is:

r_coll＝‖d_i‖

In actual motion, the motion ranges of the shafts of the mechanical arm and the self-restraint should be limited to further ensure safety, and the expression is as follows

According to the formula, the objective function of the path navigation mode is obtained as follows:

Wherein, K _t1,K_t2 is the weight coefficient of the path planning and collision avoidance task, and K _r1,K_r2 is the residual proportionality coefficient of the path planning and collision avoidance task.

In some embodiments, the current pose T _act may be acquired in real time by the position sensor of the surgical robot endoscope, and the pose T _focus reaching the target area may be manually set during the endoscope movement. The position p _c of the critical environment structure is manually set during the endoscope movement, and the nearest point p _a of the endoscope from the critical environment structure can be acquired by the position sensor of the endoscope of the external operation robot in real time.

(3) Dynamic tracking mode

For the dynamic tracking mode, the endoscope needs to track the position of other surgical instruments in the field of view in real time.

The target of the tracking task is that the robot joint of the mechanical arm reaches the target pose X _des of the tracking task from the current pose X _act of the tracking task, and the Jacobian matrix expression of the change relation of the current pose relative to the joint angle in the tracking task is as follows:

Where J _track represents the Jacobian matrix of the tracking task and δX _act represents the change in the current pose of the tracking task.

The residual expression of the tracking task is:

r_track＝log(X_desX_act ^-1)

Where r _rrack represents the residual of the tracking task, and T _act ^-1 represents the inverse matrix.

In the motion to the dynamic tracking mode, other surgical instruments need to be prevented from being collided as much as possible so as to reduce the shock risk of the mechanical arm, and in the event of accidental touch, the motion speed of the mechanical arm needs to be adjusted according to the contact force so as to reduce the collision risk. Let the mould length of the mechanical arm end force obtained in the operation be II F _end II, then avoid the jacobian matrix of the instrument collision task to be:

Wherein J _force represents a Jacobian matrix for avoiding an instrument collision task, X _act represents the current pose of a tracking task, and alpha is an adjustment coefficient.

In the motion process, the motion range and self-restraint of each shaft of the mechanical arm should be limited to further ensure the safety, and the expression is as follows

According to the formula, the objective function of the dynamic tracking mode is obtained as follows:

Wherein, K _t3,K_t4 is the weight coefficient of the tracking task and the avoiding task, K _r3 is the residual ratio coefficient of the avoiding task.

In some embodiments, the current pose T _act may be acquired in real time by the position sensor of the surgical robot endoscope, and the arm tip force F _end may be acquired in real time by the force sensor.

in this step, real-time data including information of the current pose of the endoscope and the contact force of the tip are acquired from the position sensor and the force sensor.

In the moving process, the optimal control is realized by dynamically adjusting the speed of the robot joint. As the endoscope moves, the system continuously monitors the state of the robotic arm and continuously updates the jacobian matrix and residual terms.

In the whole moving process, the system continuously optimizes the joint speed by solving the quadratic programming problem. The goal of the optimization is to minimize the objective function. The quadratic programming method can find the optimal speed of the robot joint as the control speed of the robot joint on the premise of meeting various constraint conditions, thereby realizing the efficient, stable and safe movement of the mechanical arm.

Through the mode, the flexibility and the reliability of the mechanical arm for the external operation are maintained in a complex task environment, the dynamic response can be changed in real time, the motion path is optimized, and the smooth completion of the task is ensured.

Although the specific embodiments of the present invention depict various acts or steps in a particular order, it should be understood that such acts or steps are required to be performed in the particular order shown or in sequential order, or that all illustrated acts or steps should be performed to achieve desirable results. In certain circumstances, multitasking and parallel processing may be advantageous. Likewise, while several specific implementation details are included in the above discussion, these should not be construed as limiting the scope of the present disclosure. Certain features that are described in the context of separate embodiments can also be implemented in combination in a single implementation. Conversely, various features that are described in the context of a single implementation can also be implemented in multiple implementations separately or in any suitable subcombination. The present invention is not limited to the above-mentioned embodiments, and any changes or substitutions that can be easily understood by those skilled in the art within the technical scope of the present invention are intended to be included in the scope of the present invention.

The present invention is not limited to the above-mentioned embodiments, and any changes or substitutions that can be easily understood by those skilled in the art within the technical scope of the present invention are intended to be included in the scope of the present invention.

Claims

1. An external operation robot endoscope control system based on a layered quadratic programming framework, wherein the external operation robot endoscope comprises an endoscope body, a mechanical arm, a position sensor and a force sensor, and the mechanical arm is provided with a plurality of robot joints, and the system is characterized in that the system comprises the following steps:

s4, controlling the robot joint to move based on the control speed, and finally completing the endoscopic control of the surgical robot;

In step S1:

In the pivoting motion mode, the endoscope body moves about a pivot point;

in the dynamic tracking mode, the endoscope body moves along with the instrument to be tracked;

in step S2:

2. The hierarchical quadratic programming framework-based surgical robot endoscope control system of claim 1, wherein:

In the pivot motion mode, the position sensor of the endoscopic surgical robot acquires the pivot point position in real time Calculating the position of the projection point of the pivot point on the nerve endoscope shaft according to the position of the pivot point by a geometric relation;

in the path navigation mode, the current pose of the path planning task and the nearest point position of the endoscope from a key environment structure are acquired in real time by a position sensor of the endoscope of the external surgical robot ;

In the dynamic tracking mode, the force sensor acquires the contact force of the tail end of the mechanical arm in real time.

3. The system for controlling an endoscope of a surgical robot based on a hierarchical quadratic programming framework according to claim 2, wherein the step S3 specifically comprises: