An endoscope vision and inertial navigation coordinated path planning system for a ventriculoscope instrument

Abstract

This invention discloses a collaborative path planning system for endoscopic instruments using both visual and inertial navigation, relating to the field of surgical path planning. Specifically, it includes: a data fusion module that fuses visual and encoder information to obtain the continuous motion trajectory of the instrument's end effector; a motion decomposition module that detects abrupt motion changes and segments them into basic operational units; a surgical intent recognition module that analyzes the probability distribution of the current surgical intent using a two-layer state recognition model; a trajectory prediction module that selects a prediction model based on the intent probability to generate the future trajectory and confidence level; and a path coordination module that constructs a dynamic cost map including field of view coverage cost, observation angle cost, and smooth movement cost, and solves for the desired motion command through model predictive control. This invention achieves proactive prediction and coordinated movement of the instrument by the endoscope through multi-layered intent perception, fundamentally eliminating the lag in traditional feedback control, solving the problem of instruments frequently leaving the field of view, and improving surgical fluency and safety.

Description

Technical Field

[0001] This invention relates to the field of surgical path planning, and more specifically, to a ventriculoscope instrument collaborative path planning system that combines endoscopic vision and inertial navigation. Background Technology

[0002] Neuroendoscopic surgery is an effective method for treating neurosurgical diseases such as cerebral hemorrhage, and endoscopes are widely used as important surgical instruments. Existing technologies use robotic arms to control endoscopic motion. For example, CN120053077A discloses an endoscopic control method for a neurosurgical robot based on a hierarchical quadratic programming framework, which divides endoscopic motion into three modes: pivot motion, path navigation, and dynamic tracking, and controls it through feedback from position and force sensors. This method is an error-based feedback control; the endoscope's pose is only adjusted when the instrument's end-effector deviates from the center of the field of view. This control has inherent lag, and temporary loss of vision cannot be avoided when the instrument moves rapidly.

[0003] Another existing technology, CN118830916A, discloses a preoperative planning method based on a multi-objective particle swarm optimization algorithm, which improves motion flexibility by optimizing parameters such as instrument incision, endoscopic incision, and the initial position of the robotic arm. This method belongs to preoperative static planning, and its optimization results depend on preoperative images and preset parameters. It cannot cope with the dynamic changes in the instrument operation path during surgery, and hand-eye coordination is difficult to guarantee in real time.

[0004] Therefore, how to achieve active, continuous, and precise tracking of the endoscopic field of view during the movement of the instrument, and avoid the loss of field of view due to control lag or static planning failure, has become a technical problem that urgently needs to be solved in this field. In response to the above problems, a collaborative path planning system for endoscopic vision and inertial navigation for ventriculoscope instruments is proposed. Summary of the Invention

[0005] In order to overcome the above-mentioned defects of the prior art, the embodiments of the present invention provide a ventriculoscope instrument collaborative path planning system with endoscopic vision and inertial navigation, which aims to solve the problem that the endoscopic field of view lags behind the movement of the instrument and cannot achieve active coordination, resulting in the instrument end point frequently leaving the center of the field of view.

[0006] To achieve the above objectives, the present invention provides the following technical solution: To achieve the above objectives, the present invention provides the following technical solution: A collaborative path planning system for endoscopic visual and inertial navigation ventriculoscope instruments includes: The data fusion module combines the instrument position information captured by the ventriculoscope lens with the ventriculoscope position change information captured by the joint encoder of the robotic arm holding the endoscope, and obtains the continuous motion trajectory of the instrument end effector in the ventricle space through coordinate transformation. ; The motion decomposition module, connected to the data fusion module, is used to analyze the continuous motion trajectory. The moving speed of the end effector Rotation speed and trajectory curvature The system detects abrupt changes in the moving speed, rotational speed, and trajectory curvature, and defines the trajectory segments between adjacent abrupt changes as basic operation units. Output the sequence of basic operation units; The surgical intent recognition module, connected to the motion decomposition module, incorporates a two-layer state recognition model. This model comprises an upper surgical intent layer and a lower basic operation unit layer. The surgical intent recognition module inputs the basic operation unit sequence as observation data into the two-layer state recognition model and obtains information about the current surgical stage. As a constraint for the state transition of the upper-level surgical intent layer, the probability values ​​of each intent in the upper-level surgical intent layer are updated, and the probability distribution of the current surgical intent is output. ; The trajectory prediction module, connected to the surgical intent recognition module, incorporates at least two motion prediction models. The trajectory prediction module determines the trajectory based on the probability distribution of the current surgical intent. Select a target prediction model from the motion prediction models to generate the predicted trajectory of the device's end effector within a future time window. and the confidence level of each trajectory point ; The path coordination module, connected to the trajectory prediction module, is used to select multiple candidate ventriculoscope target poses within the reachable space of the robotic arm holding the endoscope. Calculate the field of view coverage cost for each candidate target pose. The cost of observation angle and the cost of smooth movement ,in Based on the confidence level of each location point on the predicted trajectory And its projection calculation within the ventriculoscope field of view, The angle between the direction the ventriculoscope points and the direction the instrument moves, corresponding to the current surgical intent, is calculated. Based on the current ventriculoscope position Move to candidate target pose Calculation of required joint range of motion; right , and The total cost of each candidate target pose is obtained by weighted summation. A dynamic cost map is constructed, and an optimization problem is established using the dynamic cost map as the objective function. The desired motion command is then obtained by solving the problem. Send to the underlying controller of the robotic arm holding the mirror.

[0007] Furthermore, in the motion decomposition module, abrupt change points are detected by calculating the Mahalanobis distance between the motion feature data at the current moment and the historical data within the current basic operation unit. When the Mahalanobis distance exceeds a set threshold... The time point is determined to be a sudden change point.

[0008] Furthermore, the surgical intent recognition module uses a particle filtering method to update the probability values ​​of each intent in the upper surgical intent layer. By randomly generating multiple intent state particles, the weight of each particle is updated according to the degree of matching between the basic operation unit sequence and the expected operation mode in each intent state. The probability distribution of the current surgical intent is the normalized result of all particle weights.

[0009] Furthermore, in the trajectory prediction module, the motion prediction model includes a Kalman filter model suitable for linear motion and a depth trajectory generation model suitable for complex motion, wherein the depth trajectory generation model adopts a conditional variational autoencoder architecture.

[0010] Furthermore, the trajectory prediction module acquires the actual motion trajectory segment of the device end effector, calculates the error between the actual motion trajectory segment and the predicted trajectory, and adjusts the posterior distribution of the latent variables of the depth trajectory generation model online based on the error.

[0011] Furthermore, in the path coordination module, the view coverage cost The calculation method is as follows: each position point on the predicted trajectory is assigned its confidence level. As weights, they are projected onto the ventriculoscope image plane, and the confidence scores of projection points falling within the image boundaries are statistically analyzed. Calculate the total confidence level ,but .

[0012] Furthermore, the data fusion module extracts the pixel coordinates of the instrument tip in the image coordinate system from the image sequence acquired by the ventriculoscope lens. The rotation matrix of the ventriculoscope itself is obtained from the joint encoder of the robotic arm holding the endoscope. Translation vector Through the intrinsic parameter matrix Convert pixel coordinates to normalized coordinates, and combine them with depth. Three-dimensional coordinates of the end effector .

[0013] Furthermore, it also includes a surgical stage identification module, which determines the current surgical stage based on preoperative planning information or intraoperative image recognition results, and uses the surgical stage information as the current surgical stage information. Input the surgical intent recognition module.

[0014] Furthermore, the path coordination module repeatedly performs the dynamic cost map construction and optimization problem solving at fixed intervals.

[0015] Furthermore, the data fusion module, motion decomposition module, surgical intent recognition module, trajectory prediction module, and path coordination module are sequentially connected to form a closed loop. The desired motion command output by the path coordination module drives the robotic arm holding the mirror to move, generating new image information and joint encoder information, which are then fed back to the data fusion module.

[0016] The technical effects and advantages of this invention are as follows: Compared to existing technologies, this invention divides the continuous motion trajectory of the instrument's end effector into basic operational units through a motion decomposition module. Combined with a surgical intent recognition module to analyze the surgeon's operational intent, it achieves a shift from passive tracking to active collaboration. The motion decomposition module uses a sliding window statistical method to detect abrupt changes in speed, rotation speed, and trajectory curvature in real time. It defines the trajectory segments between adjacent abrupt changes as basic operational units, with each unit having an average duration of 0.5 to 1 second.

[0017] The surgical intent recognition module uses the basic operation unit sequence as observation data and updates the intent probability distribution online through a two-layer state recognition model and particle filtering. It can output the possible intent of the next unit before the current unit is completed. Since the intent output time is much earlier than the actual deviation time of the instrument movement, the system can plan the endoscope movement path in advance, fundamentally eliminating the inherent time lag in the "deviation first, correction later" approach of traditional feedback control.

[0018] Compared to existing technologies, this invention improves adaptability to different motion patterns by dynamically selecting a prediction model based on the probability distribution of the current surgical intent through a trajectory prediction module. The trajectory prediction module incorporates at least two motion prediction models, including a Kalman filter model suitable for linear motion and a depth trajectory generation model suitable for complex motion. When the probability of hematoma aspiration intent exceeds a set threshold, the system activates the Kalman filter model. This model has clear physical constraints on uniform linear motion, and the predicted trajectory conforms to the motion laws of the instrument. When the intent is unclear, the system activates a conditional variational autoencoder model, which generates multiple hypothetical future trajectories by encoding historical trajectories, expressing motion uncertainty in the form of a probability distribution. This intent-driven model selection mechanism allows the prediction method to adjust according to changes in operational intent, avoiding the insufficient adaptability of a single model when dealing with different motion patterns.

[0019] Compared to existing technologies, this invention introduces observation angle costs into a dynamic cost map through a path coordination module, achieving a match between the endoscopic observation viewpoint and the surgical intent. The path coordination module defines the calculation method for the observation angle cost based on the current surgical intent: Under the intent of tissue separation, the cost is calculated based on the angle between the endoscopic optical axis and the normal vector of the motion plane, driving the optical axis to be nearly perpendicular to the motion plane. This observation direction provides a clearer sense of depth, making it easier for doctors to judge the relative positions of instruments and tissues. Under the intent of hematoma aspiration, the cost is calculated based on the angle between the endoscopic optical axis and the motion direction vector, driving the optical axis to align with the motion direction to maintain a clear field of view in front of the aspiration head. By combining the observation angle cost, field of view coverage cost, and smooth movement cost into optimization objectives, the system can simultaneously consider the integrity of the field of view and the rationality of the observation angle in endoscopic motion planning.

[0020] Compared to existing technologies, this invention employs a model predictive control framework to perform rolling optimization of the dynamic cost map, achieving path planning while satisfying the physical constraints of the robotic arm. The path coordination module solves a finite-time domain optimization problem with a fixed control cycle. The optimization objective is the accumulation of the total cost at each time point within the future prediction time domain, with constraints including joint angle limits and joint speed limits. Because the optimization problem is decomposed into small-scale quadratic programming subproblems, the solution time for each cycle can be controlled within 10 milliseconds, meeting real-time control requirements. The introduction of smooth movement costs constrains the joint speed changes between adjacent control cycles, avoiding violent movements of the robotic arm and ensuring the smoothness of the endoscope's motion.

[0021] Compared to existing technologies, this invention utilizes an online adaptive mechanism in the trajectory prediction module, enabling the system to gradually adapt to the operating habits of different doctors. The deep trajectory generation model employs a conditional variational autoencoder architecture, where the encoder maps historical trajectories to the posterior distribution of latent variables, and the decoder generates future trajectories based on these latent variables.

[0022] Each time the system acquires a new actual trajectory segment, it updates the posterior distribution parameters of the input encoder, causing subsequent predictions to sample from the updated posterior distribution. As the surgery progresses, the accumulated observation data gradually causes the posterior distribution to converge towards the surgeon's personalized movement pattern, and the deviation between the predicted trajectory and the actual trajectory gradually decreases. This online adaptive capability allows the system to continuously optimize its prediction performance during the surgical procedure.

[0023] Compared to existing technologies, this invention achieves continuous tracking and response to the instrument's motion state through a closed-loop feedback structure formed by sequentially connecting modules. The data fusion module outputs a continuous motion trajectory, from which the motion decomposition module extracts basic operational units. The surgical intent recognition module updates the intent probability accordingly, the trajectory prediction module generates the future trajectory and confidence level, and the path coordination module solves for the desired motion command. After the command drives the robotic arm to move, new image and encoder information is generated and fed back to the data fusion module. This closed-loop structure allows each control cycle to update the endoscope's motion command based on the latest observation data, ensuring the system can respond promptly to changes in instrument motion and maintain continuous coverage of the instrument by the endoscope's field of view. Attached Figure Description

[0024] Figure 1 This is a system module framework diagram of the present invention.

[0025] Figure 2 This is a schematic diagram of the method flow of the present invention. Detailed Implementation

[0026] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0027] Example 1 As attached Figures 1 to 2 The implementation details of the endoscopic vision and inertial navigation collaborative path planning system for ventriculoscopes are as follows: The endoscopic vision and inertial navigation-based ventriculoscope instrument collaborative path planning system provided by this invention is deployed in the ventriculoscope surgical robot control system and works in conjunction with the endoscope-holding robotic arm's underlying controller, the ventriculoscope lens, and the endoscope-holding robotic arm's joint encoder. This system operates in real time during ventriculoscope surgery to address the problem of surgical instruments frequently detaching from the ventriculoscope's field of vision.

[0028] This system comprises five core modules: a data fusion module, a motion decomposition module, a surgical intent recognition module, a trajectory prediction module, and a path coordination module. These modules are sequentially connected to form a closed-loop control circuit. The entire system runs embedded on the main control computer of the surgical robot, employing a real-time operating system to ensure the real-time performance and stability of the control.

[0029] I. Data Fusion Module The data fusion module takes as input real-time image sequences captured by the ventriculoscope lens and real-time joint angle values ​​fed back by encoders at each joint of the robotic arm, and outputs the continuous motion trajectory of the surgical instrument's end effector within the ventricle. By fusing visual information with robotic arm motion information, this module provides accurate instrument position data for subsequent modules.

[0030] 1. Image acquisition and instrument tracking The ventriculoscope lens acquired images of the surgical area at a frame rate of 60fps, with a resolution of 1920×1080 pixels. A kernel correlation filter tracking algorithm was used to extract the pixel coordinates of the surgical instrument tip.

[0031] In the first frame image, the doctor manually selects the end of the instrument as the tracking target, with the selected area being 80×80 pixels in size, and extracts the gradient direction histogram features of this area as a template.

[0032] For each subsequent frame, the region most similar to the template features is searched in the current frame. The calculation is accelerated by using a circulant matrix and a fast Fourier transform to obtain the response map of the target region.

[0033] The peak position of the response map is the pixel coordinate of the instrument tip in the current image coordinate system. The peak value of the response graph serves as the tracking confidence level. The value range is [0,1].

[0034] when Re-detection is triggered periodically, and a sliding window is used to traverse the entire image for re-localization. The tracking algorithm runs at 60fps, outputting pixel coordinate pairs in real time. and confidence level .

[0035] 2. Robotic arm pose acquisition The robotic arm holding the mirror is a six-degree-of-freedom collaborative robot, with each joint equipped with an absolute encoder and a sampling frequency of 1000Hz. The joint angles collected by the encoder are denoted as follows: .

[0036] The pose of the robotic arm's end effector in the robot's base coordinate system is calculated using a forward kinematics model of the robotic arm. The forward kinematics model employs the Denavit-Hartenberg parametric method, with parameters pre-calibrated according to the robot model.

[0037] Based on the installation dimensions of the ventriculoscope, the rotation matrix of the optical center of the ventriculoscope lens in the robot's base coordinate system is obtained. Translation vector .in It is a 3×3 matrix, representing the rotation relationship between the camera coordinate system and the world coordinate system; It is a 3×1 vector representing the position of the camera's optical center in the world coordinate system.

[0038] 3. Coordinate Transformation and Trajectory Generation Transform pixel coordinates to the ventricular spatial coordinate system. First, through the intrinsic parameter matrix... Convert pixel coordinates to normalized image coordinates. Intrinsic parameter matrix. It is a 3×3 matrix containing the focal length. and the main point The value was obtained beforehand using Zhang Zhengyou's calibration method. The conversion formula is: ; in To normalize image coordinates, express The inverse matrix.

[0039] The coordinates of the instrument's end effector in the camera coordinate system are: ,in Depth represents the distance from the end of the instrument to the optical center of the camera. The initial value is estimated before the operation begins by using the known length of the instrument and the ratio of the instrument's imaging size in the image. In this embodiment, the initial depth is set to 50 mm.

[0040] During the procedure, a Kalman filter was used to recursively estimate the depth. The state vector of this filter was: ,in The depth change rate, state transition matrix Observation matrix ,in The sampling interval is seconds, and the process noise covariance is... and observation noise covariance Based on experience, this embodiment takes... , .

[0041] Observations The plane is obtained by intersecting the ray with the fixed working plane. Based on preoperative registration information, it is assumed that the instrument tip mainly moves near the floor of the ventricle, and this plane is approximately... flat.

[0042] Position of the camera's optical center in the world coordinate system Calculated from the rotation matrix and translation vector .

[0043] The ray direction in the camera coordinate system is Transform to world coordinate system The ray parameter equation is: ,make Solve for components of 0 Then the observation depth The Kalman filter is updated every frame, outputting a smoothed depth. .

[0044] Combined with the current camera pose The three-dimensional coordinates of the instrument tip in the ventricular spatial coordinate system for: ; Using a second Kalman filter Smoothing is performed. The state vector of this filter is... ,in These are the velocity components in three directions.

[0045] State transition matrix Observation matrix Process noise covariance Observation noise covariance The smoothed instrument end-effector trajectory points output by the filter The sampling frequency is 60Hz. This is the continuous motion trajectory output by this module, which will be used as the input for subsequent modules.

[0046] II. Motion Decomposition Module The motion decomposition module receives the continuous motion trajectory of the instrument's end effector output by the data fusion module. The output is a sequence of basic operational units representing the basic units of motion of the device. The time interval between adjacent trajectory points is also included. Second.

[0047] 1. Calculation of motion characteristics For each moment Calculate the following three motion characteristics: movement speed : Indicates the speed at which the instrument's end cap moves; the calculation formula is: ; in This represents the Euclidean norm of a vector, expressed in mm / s.

[0048] Rotation speed This indicates the rate of change in the pointing direction of the instrument. First, calculate the pointing direction vector of the instrument's end. The instrument tip and the pre-marked instrument entry point The connection is determined. The instrument entry point, i.e., the skull drilling point, has the following coordinates. Obtained through preoperative registration and calibration. Therefore: ; The rotational speed is: ; in Represents the dot product of two vectors. It is the inverse cosine function, with units of rad / s.

[0049] trajectory curvature : Indicates the degree of curvature of the trajectory. (Through) , , Fit a circular arc using three points and calculate the radius of curvature. ,but: ; When three points are collinear Infinity, take The unit is mm. .

[0050] 2. Detection of sudden change time Set the length of the sliding window A frame stores historical motion feature data within the current basic operation unit. The feature vector at each time step is... , Calculate the mean vector of features within the window. Covariance Matrix : ; ; in The actual number of data points within the window, when hour Take the actual value.

[0051] To measure current features To calculate the difference between historical features and Mahalanobis distance: ; when When the covariance matrix is ​​singular, Euclidean distance should be used instead; if If the matrix is ​​close to singular, singular value decomposition is used to find the pseudo-inverse instead of the inverse matrix.

[0052] Set threshold ,when Time determination The time is the point of sudden change.

[0053] 3. Generation of basic operation units Adjacent mutation time points and A trajectory segment between two points is defined as a basic operation unit. Each basic operational unit uses the mean vector of motion features within that segment. Covariance Matrix Characterization. The motion decomposition module outputs the currently accumulated basic operating unit parameters in real time at a frequency of 60Hz, and when a sudden change is detected, it updates the completed units. Send to the surgical intent recognition module.

[0054] III. Surgical Intent Recognition Module The surgical intent recognition module receives the basic operation unit sequence output by the motion decomposition module. and the current surgical stage output by the surgical stage identification module. The output is the probability distribution of the current surgical intention. This module incorporates a hierarchical hidden Markov model, obtained through offline training.

[0055] 1. Model Structure The upper intent layer contains surgical intention status: Organizational separation Hematoma aspiration Lesion removal Hemostasis other.

[0056] The lower primitive layer contains The basic operational unit types are identified. K-means clustering is used to divide the basic operational units in the training data into 8 classes, each using a Gaussian distribution. describe, Prior probabilities of various types This represents the frequency of occurrence of each category in the training data.

[0057] The probability of transfer between intentions is affected by the surgical stage Restraint. The surgical phase is divided into three categories: Hematoma removal Hemostasis Lesion removal.

[0058] Intent transition matrix at different stages The result is obtained through maximum likelihood estimation and then smoothed using Laplace to avoid zero probability. The smoothing coefficient is set to 1.

[0059] Observation probability This represents the distribution of basic operation unit types under each intent, with parameters estimated from the training data.

[0060] 2. Surgical Stage Recognition Module The surgical stage identification module obtains the current surgical stage in two ways: when there is a preoperative planning document, it parses the surgical step markers in the document; When no preoperative planning document is available, a U-Net-based image segmentation network is used to identify the surgical stage in real time. The network takes a ventriculoscopy image as input and outputs a probability distribution of the surgical stages, selecting the stage with the highest probability as the current surgical stage. The module outputs at a frequency of 1Hz.

[0061] 3. Particle Filtering Online Inference Particle filtering is used to update the posterior probability of intent and the number of particles in real time. Each particle Carry an intention state and weight .

[0062] Initialization: Based on the initial surgical stage From the prior distribution Medium sampling There are 1 particle, with an initial weight of 1. .

[0063] For each newly observed basic operational unit : Prediction steps: For each particle According to the current intention and the current surgical stage From the transition matrix New intentions in sampling .

[0064] Weight update: Let The mean vector is Then the observed likelihood is: ; in express In the Probability density values ​​under a Gaussian-like distribution For the purpose Next The probability of the primitive class. Update particle weights: ; Normalized weights .

[0065] Resampling: Calculating the effective number of particles .when System resampling is performed periodically, high-weight particles are copied, low-weight particles are discarded, and the weights of all particles are reset. .

[0066] The probability distribution of the current surgical intent is as follows: ; in This is an indicator function.

[0067] The surgical intent recognition module outputs the intent probability distribution at the basic operation unit update frequency. .

[0068] IV. Trajectory Prediction Module The trajectory prediction module receives the current intent probability distribution. and historical trajectory ,in The frame outputs the predicted trajectory of the device's end effector over the next 1.5 seconds, along with the confidence level for each trajectory point. The prediction model is selected based on the intent probability.

[0069] 1. Three prediction models Kalman filter model: suitable for hematoma aspiration. It assumes the instrument moves at a constant velocity in a straight line, and the state vector... The trajectory points for each frame within the next 1.5 seconds are predicted recursively using standard Kalman filtering. and its covariance Confidence level Normalize to the [0,1] interval.

[0070] Interactive multi-model filtering model: Suitable for tissue separation purposes, it simultaneously runs a uniform speed model and a uniform turning model, obtaining prediction results through interactive, filtering, and fusion steps. The transition probability matrix between models is preset as follows. .

[0071] Conditional variational autoencoder model: suitable for situations where the intent is unclear, adopts a CVAE architecture, and both the encoder and decoder are two-layer LSTM.

[0072] The encoder encodes historical trajectories into latent variables. The mean and logarithmic variance; the decoder uses and initial state Given the conditions, generate the trajectory for the next 90 frames. During online runtime, starting from the prior... Twenty samples were sampled to generate 20 possible trajectories, and then a Gaussian mixture model was used to fit the probability distribution of the trajectories. ; in Second, It is a 3-dimensional vector. It is a 3×3 matrix.

[0073] 2. Model Selection Strategy If the probability of hematoma aspiration is low We choose the Kalman filter model.

[0074] If the probability of the organization's intention to separate is high Select the interactive multi-model filtering model.

[0075] If all intent probabilities are below 0.5, choose the conditional variational autoencoder model.

[0076] 3. Online Adaptive For the conditional variational autoencoder model, each new actual trajectory segment is obtained ( (Frame), input the new observed trajectory into the encoder to obtain the updated posterior distribution of the latent variables. Subsequent predictions will be made from... Sampling. Every 5 cumulative updates, the model is fine-tuned using the new data.

[0077] V. Path Coordination Module The path coordination module receives the predicted trajectory probability distribution output by the trajectory prediction module and the current surgical intent output by the surgical intent recognition module, and outputs the desired motion command of the robotic arm holding the scope.

[0078] 1. Candidate pose sampling At the current joint angle Centered on the sample, sampling is performed in each dimension with a step size of 5° to generate Candidate joint angles Candidate points exceeding joint limits are eliminated. The corresponding ventriculoscope pose is calculated using forward kinematics. That is, rotation matrix Translation vector .

[0079] 2. Cost Calculation Cost of field of view coverage : Predict trajectory points ( The projection is onto the image plane corresponding to the candidate pose. The projection formula is: ; If a projected point falls within the image boundary, then that point is covered by the field of view. Each trajectory point has a confidence level. Define the total confidence level. Coverage confidence level and .Will Defined as the sum of uncovered confidence scores: ; Cost of observation angle Calculated based on the current surgical intent I.

[0080] If the intention is tissue separation, it is desirable that the optical axis of the ventriculoscope is perpendicular to the plane of instrument motion, and the normal vector of the plane of motion is... Obtained from principal component analysis of the trajectory points in the most recent 15 frames. Optical axis direction. for The third column. Then: ; If the intention is hematoma aspiration, it is desirable that the optical axis aligns with the direction of instrument movement, and the direction of movement vector... The direction of the average velocity of the trajectory points in the most recent 5 frames is determined. Therefore: ; For other intentions Take 0.5.

[0081] Smooth movement cost Calculate the normalized joint space distance from the current joint angle to the candidate joint angle: ; 3. Dynamic Cost Map The total cost of each candidate pose is the weighted sum of the three costs mentioned above: ; Weighting , , Iterate through all candidate poses and calculate the value of each. The candidate poses and their costs are stored as a dynamic cost map.

[0082] 4. Solving Model Predictive Control Set the prediction time domain Seconds, controlling the time domain Seconds, control cycle Seconds. In each control cycle With current pose Starting with the dynamic cost map as the objective function and the kinematic model of the robotic arm as the constraint, an optimization problem is constructed.

[0083] Define the state variable as the ventriculoscope pose. Control input Joint velocity The system model is as follows: ; in We obtain the following through the integration of forward kinematics and joint velocities: Then by Calculations based on forward kinematics .

[0084] The optimization problem is: ; The constraints include the state transition equation, joint angle limits, and joint velocity limits. The regularization coefficient is... This represents the maximum speed of each joint.

[0085] The solution is obtained using a sequential quadratic programming algorithm, with a maximum of 50 iterations and a convergence tolerance. The optimal control sequence is obtained by solving the problem. The first control command The desired motion command is sent to the underlying robotic arm controller for execution.

[0086] VI. System Initialization and Exception Handling 1. Initialization process When the system starts up, each module needs to accumulate a certain amount of data before it can function properly: Data fusion module: Waits for the first 30 frames of images to fill the Kalman filter history buffer, and outputs the raw observations before that.

[0087] Motion decomposition module: Waits for the first 15 frames of data to fill the sliding window. Before that, mutation detection is not active and no basic operation unit is output.

[0088] The trajectory prediction module waits for the first 90 frames of historical trajectories; predictions cannot be made before this time. When data is insufficient, the path coordination module only considers... and , Set to 0.

[0089] Surgical intent recognition module: Initializes normally based on sampled particles from the initial surgical phase.

[0090] 2. Asynchronous data processing Each module operates at a different frequency, and the latest data is delivered through a publish-subscribe pattern: The data fusion module outputs at 60Hz. .

[0091] The motion decomposition module updates at 60Hz, but only sends data when the basic operation unit completes. .

[0092] The surgical intent recognition module outputs updates at a basic operation unit frequency. .

[0093] The trajectory prediction module is triggered either when the intent is updated or every 0.5 seconds.

[0094] The path coordination module runs at 20Hz, reading the latest predicted trajectory and intent each time.

[0095] 3. Exception Handling Visual tracking lost: like If the frame rate exceeds 10 frames, a re-detection will be triggered. If the re-detection fails, pause the path coordination module and issue an audible alarm. After successful re-detection, reset the states of the depth Kalman filter and the location Kalman filter to the current observation values, and restart the path coordination module.

[0096] Insufficient motion decomposition data: When data points within the window... At that time, the Mahalanobis distance was replaced by the Euclidean distance.

[0097] The effective number of particles in the particle filter is too low: If The particle count was temporarily increased to 500 and resampling was performed.

[0098] If the MPC solution does not converge: If the SQP iteration fails to converge after more than 50 iterations, take the best feasible solution of the current iteration; if there is no feasible solution, retain the control instructions of the previous cycle.

[0099] VII. Closed-loop control process The entire system operates in a 20Hz cycle. The execution sequence within each cycle is as follows: 1. The data fusion module reads the latest image and encoder data, performs coordinate transformation and smoothing filtering, and outputs the current instrument end position. .

[0100] 2. The motion decomposition module is based on Update motion features and detect any abrupt changes. If a change is detected, complete the current basic operation unit and send it to the surgical intent recognition module; otherwise, continue accumulating.

[0101] 3. If the surgical intent recognition module receives a new basic operation unit... Then, particle filtering is performed to update the output and a new intention probability distribution is generated. .

[0102] 4. The trajectory prediction module determines whether the prediction needs to be updated: If the intent probability has been updated, or if more than 0.5 seconds have passed since the last prediction, the predicted trajectory and the confidence scores of each point are regenerated based on the latest intent and historical trajectories. .

[0103] 5. The path coordination module reads the latest predicted trajectory and intent, performs candidate pose sampling and cost calculation, constructs a dynamic cost map, and obtains the optimal control command through MPC. .

[0104] 6. Control Commands The data is sent to the underlying PID controller to drive the movement of the robotic arm holding the mirror, generating new image and joint encoder data, which is then fed back to the data fusion module to start the next cycle.

[0105] Through the aforementioned real-time closed-loop control, the ventriculoscope can actively adjust its position and posture according to the movement trend of the instrument, so that the end of the instrument is always near the center of the field of vision.

[0106] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A collaborative path planning system for endoscopic visual and inertial navigation instruments in ventriculoscopes, characterized in that, include: The data fusion module combines the instrument position information captured by the ventriculoscope lens with the ventriculoscope position change information captured by the joint encoder of the robotic arm holding the endoscope, and obtains the continuous motion trajectory of the instrument end effector in the ventricle space through coordinate transformation. ; The motion decomposition module, connected to the data fusion module, is used to analyze the continuous motion trajectory. The moving speed of the end effector Rotation speed and trajectory curvature The system detects abrupt changes in the moving speed, rotational speed, and trajectory curvature, and defines the trajectory segments between adjacent abrupt changes as basic operation units. Output the sequence of basic operation units; The surgical intent recognition module, connected to the motion decomposition module, incorporates a two-layer state recognition model. This model comprises an upper surgical intent layer and a lower basic operation unit layer. The surgical intent recognition module inputs the basic operation unit sequence as observation data into the two-layer state recognition model and obtains information about the current surgical stage. As a constraint for the state transition of the upper-level surgical intent layer, the probability values ​​of each intent in the upper-level surgical intent layer are updated, and the probability distribution of the current surgical intent is output. ; The trajectory prediction module, connected to the surgical intent recognition module, incorporates at least two motion prediction models. The trajectory prediction module determines the trajectory based on the probability distribution of the current surgical intent. Select a target prediction model from the motion prediction models to generate the predicted trajectory of the device's end effector within a future time window. and the confidence level of each trajectory point ; The path coordination module, connected to the trajectory prediction module, is used to select multiple candidate ventriculoscope target poses within the reachable space of the robotic arm holding the endoscope. Calculate the field of view coverage cost for each candidate target pose. The cost of observation angle and the cost of smooth movement ,in Based on the confidence level of each location point on the predicted trajectory And its projection calculation within the ventriculoscope field of view, The angle between the direction the ventriculoscope points and the direction the instrument moves, corresponding to the current surgical intent, is calculated. Based on the current ventriculoscope position Move to candidate target pose Calculation of required joint range of motion; right , and The total cost of each candidate target pose is obtained by weighted summation. A dynamic cost map is constructed, and an optimization problem is established using the dynamic cost map as the objective function. The desired motion command is then obtained by solving the problem. Send to the underlying controller of the robotic arm holding the mirror.

2. The endoscopic vision and inertial navigation collaborative path planning system for ventriculoscope instruments according to claim 1, characterized in that, In the motion decomposition module, abrupt change points are detected by calculating the Mahalanobis distance between the current motion feature data and the historical data within the current basic operation unit. When the Mahalanobis distance exceeds a set threshold... The time point is determined to be a sudden change point.

3. The endoscopic vision and inertial navigation collaborative path planning system for ventriculoscope instruments according to claim 1, characterized in that, The surgical intent recognition module uses a particle filtering method to update the probability values ​​of each intent in the upper surgical intent layer. By randomly generating multiple intent state particles, the weight of each particle is updated according to the degree of matching between the basic operation unit sequence and the expected operation mode in each intent state. The probability distribution of the current surgical intent is the normalized result of all particle weights.

4. The endoscopic vision and inertial navigation collaborative path planning system for ventriculoscope instruments according to claim 1, characterized in that, In the trajectory prediction module, the motion prediction model includes a Kalman filter model suitable for linear motion and a depth trajectory generation model suitable for complex motion. The depth trajectory generation model adopts a conditional variational autoencoder architecture.

5. The endoscopic vision and inertial navigation collaborative path planning system for ventriculoscope instruments according to claim 4, characterized in that, The trajectory prediction module acquires the actual motion trajectory segment of the device end effector, calculates the error between the actual motion trajectory segment and the predicted trajectory, and adjusts the posterior distribution of the latent variables of the depth trajectory generation model online based on the error.

6. The endoscopic vision and inertial navigation collaborative path planning system for ventriculoscope instruments according to claim 1, characterized in that, In the path coordination module, the view coverage cost The calculation method is as follows: each position point on the predicted trajectory is assigned its confidence level. As weights, they are projected onto the ventriculoscope image plane, and the confidence scores of projection points falling within the image boundaries are statistically analyzed. Calculate the total confidence level ,but .

7. The endoscopic vision and inertial navigation collaborative path planning system for ventriculoscope instruments according to claim 1, characterized in that, The data fusion module extracts the pixel coordinates of the instrument tip in the image coordinate system from the image sequence acquired by the ventriculoscope lens. The rotation matrix of the ventriculoscope itself is obtained from the joint encoder of the robotic arm holding the endoscope. Translation vector Through the intrinsic parameter matrix Convert pixel coordinates to normalized coordinates, and combine them with depth. Three-dimensional coordinates of the end effector .

8. The endoscopic vision and inertial navigation collaborative path planning system for ventriculoscope instruments according to claim 1, characterized in that, It also includes a surgical stage identification module, which determines the current surgical stage based on preoperative planning information or intraoperative image recognition results, and uses the surgical stage information as the current surgical stage information. Input the surgical intent recognition module.

9. The endoscopic vision and inertial navigation collaborative path planning system for ventriculoscope instruments according to claim 1, characterized in that, The path coordination module repeatedly performs the dynamic cost map construction and optimization problem solving at fixed intervals.

10. The endoscopic vision and inertial navigation collaborative path planning system for ventriculoscope instruments according to any one of claims 1 to 9, characterized in that, The data fusion module, motion decomposition module, surgical intent recognition module, trajectory prediction module, and path coordination module are sequentially connected to form a closed loop. The desired motion command output by the path coordination module drives the robotic arm holding the mirror to move, generating new image information and joint encoder information, which are then fed back to the data fusion module.

Images