A surgical robot trajectory localization method based on parallelogram set membership estimation

Abstract

The application discloses a surgical robot trajectory positioning method based on parallelogram set member estimation and belongs to the field of multi-sensor fusion. The NDI sensor, a surgical robot and an instrument are calibrated, and position information of a double-NDI robot end at k time and position information of a marker ball are collected. A sensor bounded noise model of a parallelogram envelope, a robot trajectory model and an observation model are established. Whether the sensor information is lost is judged. If the sensor information is not lost, intersection solving is performed through a parallelogram intersection fusion equation. If the sensor information is lost, original envelope set information is maintained for iteration. After that, the fusion set is optimized, and a minimum area parallelogram envelope set is solved. The parallelogram envelope set, state estimation value and set area are output. The application overcomes the positioning information loss problem caused by intraoperative occlusion and the like, realizes accurate modeling of an intraoperative optical positioning sensor under bounded noise through a parallelogram envelope method, and improves the intraoperative robot trajectory positioning precision.

Description

Technical Field

[0001] This invention belongs to the field of multi-sensor data fusion, and specifically designs a surgical robot trajectory localization method based on parallelogram set member estimation. Background Technology

[0002] With the rapid development of robotics technology, orthopedic surgical robots are widely used in minimally invasive osteotomy due to their smaller incisions, less bleeding, and faster postoperative recovery. To ensure the safety and repeatability of minimally invasive osteotomy, extremely high-precision trajectory tracking is required, ensuring repeatability and navigation accuracy of less than 1 millimeter. Although osteotomy surgical robots possess self-localization capabilities, their own localization information cannot be integrated with the complex surgical environment, leading to positioning deviations during the operation. Therefore, surgical robot localization requires state estimation and positioning based on information from external sensors.

[0003] Commonly used state estimation methods are divided into probabilistic filtering estimation algorithms and set-membership state estimation algorithms. Probabilistic filtering estimation largely depends on accurate information about the noise distribution. When the noise does not meet the assumed distribution conditions or the sampling information is insufficient, the accuracy of state estimation will be severely affected. In fact, intraoperative sensor noise has an unknown but bounded distribution, making it difficult to accurately model intraoperative noise using a specific distribution. Furthermore, sensor drift, occlusion, and missing information at a certain moment can also cause probabilistic filtering estimation to diverge, affecting the robot position estimation results. Therefore, introducing set-membership estimation methods that model unknown but bounded noise into intraoperative robot trajectory estimation can avoid estimation bias caused by missing data and effectively improve the accuracy of surgical robot position estimation.

[0004] Most existing robot state set estimation methods are based on ellipsoidal envelopes to describe the state set. They update and estimate robot trajectory state values ​​by fusing localization information from multiple observation sets and state sets, and optimize to find the minimum state estimation set as the optimal solution for the true value. However, ellipsoidal envelope-based methods introduce a lot of redundant information in the process of modeling the true state value and bounded noise, and intersection optimization is prone to divergence. When fusing multiple ellipsoids, it is difficult to obtain a uniquely determined minimum volume ellipsoid. Suboptimal fusion results in slow convergence and large bias in robot trajectory estimation, limiting the accuracy of multi-sensor localization trajectory estimation. Summary of the Invention

[0005] To address the technical problems mentioned in the background, this invention proposes a surgical robot trajectory localization method based on parallelogram set member estimation. This method overcomes localization deviations caused by intraoperative sensor malfunctions and NDI observation drift through a multi-observation set member estimation algorithm. It utilizes a bounded noise modeling method to realize the full envelope of the parallelogram set of the localization truth, reducing redundant information introduced by the noise set in the original envelope. Furthermore, it employs a parallelogram intersection fusion equation to perform intersection and optimization on the parallelogram set under the set member framework to solve the non-convergence problem of set member estimation optimization, thereby improving the localization accuracy of the osteotomy robot.

[0006] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0007] A surgical robot trajectory localization method based on parallelogram set member estimation includes the following steps:

[0008] (1) Calibrate two NDI sensors so that the robot's marked point operating range is within the observation range of the NDI sensors, and construct an observation model Y at any time k based on the planned trajectory and sensor parameters. k ;

[0009] (2) Initialize the robot trajectory state value x0, and estimate the state value x0 based on the robot's motion trajectory. k Perform motion state modeling and predict the state estimate at any time. Obtain the predicted state set And solve for the predicted state estimate set at any time. The parallelogram outer envelope yields the predicted state set. outer envelope set

[0010] (3) Collect the NDI sensor observation position information at any time, and determine whether the observation position information is missing. If it is missing, the true state value at the next time moment is assumed to be the predicted estimate obtained by solving the predicted state set, and return to step (2); if it is not missing, the consistent state set represented by the observation is obtained by inverse solving using the observation model and observation information. and the corresponding outer envelope

[0011] (4) The consistent state set obtained in step (3) The predicted state set obtained in step (2) Perform intersection fusion and solve for the set of all intersections that meet the conditions after fusion. and the corresponding outer envelope set That is, to find the solution set of all states that satisfy the conditions;

[0012] (5) Optimize the solution set of the state estimation set, solve for the minimum area state estimation set, and represent the corresponding optimal outer envelope set as P. k (X k ), P k (X k The estimated true value represented by the midpoint is input into step (2) and used as the robot trajectory state value at the next moment for iteration to calculate the state prediction set at the next moment;

[0013] (6) The optimal outer envelope set P corresponding to the output state set k (X k ), State estimation set X k State estimate x k and the area of ​​the set of states.

[0014] Furthermore, the NDI sensor error boundary condition in step (1) is: An observation equation y is constructed based on the relationship between the robot trajectory and the NDI sensor observations. k =Cx k +v k , where x k Let v be the state value at time k, C be the observation matrix, and v be the state value at time k. k Let be the NDI sensor noise at time k; where, This is the set value.

[0015] Furthermore, step (2) specifically involves:

[0016] The robot's state equations are constructed as follows:

[0017]

[0018] Where, x k-1 Let u be the robot's state value at time k-1. k-1 Let w be the trajectory interpolation value of the surgical robot at time k-1 in the trajectory estimation, and let w be a constant. k-1 The state noise at time k-1 satisfies the bounded condition. For x k-1 The predicted state estimate for the next time step. As set values, A and B are equation coefficients;

[0019] Then solve for the predicted estimate. The outer envelope of the parallelogram:

[0020] In surgical robot trajectory planning, a position coordinate system is established on the trajectory osteotomy plane. The envelope prediction estimate is obtained by dynamically translating two non-parallel straight lines within the position coordinate system. Parallelogram P(θ,X0,E):

[0021]

[0022] In the formula, θ = (θ1, θ2), X0 = (x0, y0), E = (e1, e2); θ = (θ1, θ2) are the angles between the two non-parallel lines and the vertical axis of the coordinate system, (x, y) are the intersection points of the two non-parallel lines and the vertical axis, e1, e2 are the distances from the midline of the two non-parallel lines of parallelogram P(θ, X0, E), and (x0, y0) is the center point of parallelogram P(θ, X0, E), which is also the estimated state value predicted at the corresponding time.

[0023] Therefore, all predicted estimates The set of predicted states formed The set of outer envelope parallelograms is represented as:

[0024]

[0025] In the formula, For the set of predicted states The set of distances from two sides to the median of all parallelograms whose outer envelope is in the given set. For the set of predicted states The set of outer envelope rotation angles of all outer envelope parallelograms in the set.

[0026] Furthermore, in step (3), the uniform state set represented by the observations is obtained by inverse solving using the observation model and observation information. and the corresponding outer envelope The specific process is as follows:

[0027] The consistent state is obtained by solving the problem using the observation model and the position information collected by the NDI sensor. and consistent state set

[0028]

[0029]

[0030] The parallelogram envelope method is used to describe the set of uniform states:

[0031]

[0032] In the formula, Let x′ be the set of uniform states with an irregular envelope. k It is the center of the set of parallelograms in a uniform state. The distance from the boundary of the uniform state set to the center line. C represents the parallelogram outer envelope parameter of the uniform state equation.k For the observation matrix, V represents the observation noise boundary. k This represents a set of noise.

[0033] Furthermore, the specific process of step (4) is as follows:

[0034] Using the predicted state set and consistent state set The state truth information contained therein consists of the intersection set of all conditions satisfied after fusion, obtained from both the model and observation perspectives. The process of merging the intersection of the two sets satisfies the following equation:

[0035]

[0036] Where, x k Let k be the true value of the state at time k. Let x be the state truth value. k To solve for the truth estimation set at the next time step, the irregular envelope set needs to be enveloped into a parallelogram-shaped state estimation set.

[0037]

[0038]

[0039] In the formula, Let be the predicted estimate set of the parallelogram envelope obtained from the state set at time k-1. Let P(conv(X) be the set of consistent states of the regular envelope. k )) is the set of state estimates for the parallelogram envelope;

[0040] Predict the state set P(θ) for a parallelogram k ,x k ,e k and the set of consistent states Solving for the intersection yields the following fusion equation:

[0041]

[0042] In the formula, θ = (θ1, θ2) is the set of states P(θ). k ,x k ,e k ) parameter θ k (x, y) represents the state set parameters x. k λ represents the parameters of different area estimation sets that satisfy the fusion condition, and P λ (θ′ k ,x′ k ,e′k ,λ) represents the parameterized state estimate after fusion, θ′ k ,x′ k ,e′ k Let represent the parameters of the parallelogram group that meets the conditions after the intersection is merged, where the truth value must be enveloped by the solution set in each set group;

[0043] Solving the above intersection equation yields:

[0044]

[0045]

[0046]

[0047] In the solution of parameter θ, it is expressed as p(θ)=θ·I2, where I2 is a two-dimensional identity matrix.

[0048] Furthermore, in step (5), the solution set of the state estimation set is optimized to solve for the minimum area state estimation set, and the corresponding optimal outer envelope set is represented as P. k (X k Specifically:

[0049]

[0050] Where λ is the parameter of the estimated set solution group in step (4), and n represents the dimension of the state value.

[0051] The beneficial effects of adopting the above technical solution are as follows:

[0052] This invention overcomes the problems of excessive positioning errors caused by intraoperative visual sensor positioning point drift, noise interference, lens occlusion, and information loss. Furthermore, it reduces redundant estimation information introduced by fusion and optimization within the framework of the ensemble, thereby improving the accuracy of intraoperative robot trajectory estimation. Attached Figure Description

[0053] Figure 1 This is a basic flowchart of the present invention. Detailed Implementation

[0054] The technical solution of the present invention will be described in detail below with reference to the accompanying drawings.

[0055] This invention designs a surgical robot trajectory localization method based on parallelogram set membership estimation, such as... Figure 1 As shown, the steps are as follows:

[0056] Step 1: Calibrate two NDI sensors to ensure the robot's marked points operate within the NDI sensor's observation range, and construct an observation model Y at any time k based on the planned trajectory and sensor parameters.k ;

[0057] Step 2: Initialize the robot trajectory state value x0, and estimate the state value x based on the robot's motion trajectory. k Perform motion state modeling and predict the state estimate at any time. Obtain the predicted state set And solve for the predicted state estimate set at any time. The parallelogram outer envelope yields the predicted state set. outer envelope set

[0058] Step 3: Collect the NDI sensor observation location information at any time, and determine whether the observation location information is missing. If it is missing, the true state value at the next time moment is assumed to be the predicted estimate obtained by solving the predicted state set, and return to step (2); if there is no missing information, the consistent state set represented by the observation is obtained by inverse solving using the observation model and observation information. and the corresponding outer envelope

[0059] Step 4: Combine the consistent state set obtained in step (3) The predicted state set obtained in step (2) Perform intersection fusion and solve for the set of all intersections that meet the conditions after fusion. and the corresponding outer envelope set That is, to find the solution set of all states that satisfy the conditions;

[0060] Step 5: Optimize the solution set of the state estimation set, solve for the minimum area state estimation set, and represent the corresponding optimal outer envelope set as P. k (X k ), P k (X k The estimated true value represented by the midpoint is input into step (2) and used as the robot trajectory state value at the next moment for iteration to calculate the state prediction set at the next moment;

[0061] Step 6: Output the optimal outer envelope set P corresponding to the output state set. k (X k ), State estimation set X k State estimate x k and the area of ​​the set of states.

[0062] In this embodiment, step 1 above can be implemented using the following preferred solution:

[0063] Based on the NDI sensor error boundary condition |v k |≤ε vk The relationship between robot trajectory and observed values ​​is used to construct observation equations.

[0064] y k =Cx k +v k

[0065] Where, x k Let v be the state value at time k, C be the observation matrix, and v be the state value at time k. k Let k be the NDI sensor noise. This is the set value.

[0066] In this embodiment, step 2 above can be implemented using the following preferred solution:

[0067] The robot's state equations are constructed as follows:

[0068]

[0069] Where, x k-1 Let u be the robot's state value at time k-1. k-1 Let w be the trajectory interpolation value of the surgical robot at time k-1 in the trajectory estimation, and let w be a constant. k-1 The state noise at time k-1 satisfies the bounded condition. For x k-1 The predicted state estimate for the next time step. Here, A and B are set values, and they are equation coefficients.

[0070] Then solve for the predicted estimate. The outer envelope of the parallelogram:

[0071] In surgical robot trajectory planning, a position coordinate system is established on the trajectory osteotomy plane. The envelope prediction estimate is obtained by dynamically translating two non-parallel straight lines within the position coordinate system. Parallelogram P(θ,X0,E):

[0072]

[0073] In the formula, θ = (θ1, θ2), X0 = (x0, y0), E = (e1, e2); θ = (θ1, θ2) are the angles between the two non-parallel lines and the vertical axis of the coordinate system, (x, y) are the intersection points of the two non-parallel lines and the vertical axis, e1, e2 are the distances from the midline of the two non-parallel lines of parallelogram P(θ, X0, E), and (x0, y0) is the center point of parallelogram P(θ, X0, E), which is also the estimated state value predicted at the corresponding time.

[0074] Therefore, all predicted estimates The set of predicted states formed The set of outer envelope parallelograms is represented as:

[0075]

[0076] In the formula, For the set of predicted states The set of distances from two sides to the median of all parallelograms whose outer envelope is in the given set. For the set of predicted states The set of outer envelope rotation angles of all outer envelope parallelograms in the set.

[0077] In this embodiment, step (3) above uses the observation model and observation information to inversely solve for the set of uniform states represented by the observations. and the corresponding outer envelope The specific process is as follows:

[0078] The consistent state is obtained by solving the problem using the observation model and the position information collected by the NDI sensor. and consistent state set

[0079]

[0080]

[0081] The parallelogram envelope method is used to describe the set of uniform states:

[0082]

[0083] In the formula, Let x′ be the set of uniform states with an irregular envelope. k It is the center of the set of parallelograms in a uniform state. The distance from the boundary of the uniform state set to the center line. C represents the parallelogram outer envelope parameter of the uniform state equation. k For the observation matrix, V represents the observation noise boundary. k This represents a set of noise.

[0084] In this embodiment, step 4 above can be implemented using the following preferred solution:

[0085] Using the predicted state set and consistent state set The state truth information contained therein consists of the intersection set of all conditions satisfied after fusion, obtained from both the model and observation perspectives. The process of merging the intersection of the two sets satisfies the following equation:

[0086]

[0087] Where, x k Let k be the true value of the state at time k. Let x be the state truth value. k To solve for the truth estimation set at the next time step, the irregular envelope set needs to be enveloped into a parallelogram-shaped state estimation set.

[0088]

[0089]

[0090] In the formula, Let be the predicted estimate set of the parallelogram envelope obtained from the state set at time k-1. Let P(conv(X) be the set of consistent states of the regular envelope. k )) is the set of state estimates for the parallelogram envelope;

[0091] Predict the state set P(θ) for a parallelogram k ,x k ,e k and the set of consistent states Solving for the intersection yields the following fusion equation:

[0092]

[0093] In the formula, θ = (θ1, θ2) is the set of states P(θ). k ,x k ,e k ) parameter θ k (x, y) represents the state set parameters x. k λ represents the parameters of different area estimation sets that satisfy the fusion condition, and P λ (θ′ k ,x′ k ,e′ k ,λ) represents the parameterized state estimate after fusion, θ′ k ,x′ k ,e′ k Let represent the parameters of the parallelogram group that meets the conditions after the intersection is merged, where the truth value must be enveloped by the solution set in each set group;

[0094] Solving the above intersection equation yields:

[0095]

[0096]

[0097]

[0098] In the solution of parameter θ, it is expressed as p(θ)=θ·I2, where I2 is a two-dimensional identity matrix.

[0099] In this embodiment, in step 5 above, the solution set of the state estimation set is optimized to solve for the minimum area state estimation set, and the corresponding optimal outer envelope set is represented as P. k (X k Specifically:

[0100]

[0101] Where λ is the parameter of the estimated set solution group in step (4), and n represents the dimension of the state value.

[0102] The above embodiments are merely illustrative of the technical concept of the present invention and should not be construed as limiting the scope of protection of the present invention. Any modifications made to the technical solutions based on the technical concept proposed in this invention shall fall within the scope of protection of this invention.

Claims

1. A surgical robot trajectory localization method based on parallelogram set member estimation, characterized in that, Includes the following steps: (1) Calibrate two NDI sensors so that the robot's marked point operating range is within the observation range of the NDI sensors, and construct an observation model Y at any time k based on the planned trajectory and sensor parameters. k ; (2) Initialize the robot trajectory state value x0, and estimate the state value x0 based on the robot's motion trajectory. k Perform motion state modeling and predict the state estimate at any time. Obtain the predicted state set And solve for the predicted state estimate set at any time. The parallelogram outer envelope yields the predicted state set. outer envelope set (3) Collect the NDI sensor observation position information at any time, and determine whether the observation position information is missing. If it is missing, the true state value at the next time moment is assumed to be the predicted estimate obtained by solving the predicted state set, and return to step (2); if it is not missing, the consistent state set represented by the observation is obtained by inverse solving using the observation model and observation information. and the corresponding outer envelope (4) The set of consistent states obtained in step (3) The predicted state set obtained in step (2) Perform intersection fusion and solve for the set of all intersections that meet the conditions after fusion. and the corresponding outer envelope set That is, to find the solution set of all state estimates that satisfy the conditions; (5) Optimize the solution set of the state estimation set, solve for the minimum area state estimation set, and represent the corresponding optimal outer envelope set as P. k (X k ), P k (X k The estimated true value represented by the midpoint is input into step (2) and used as the robot trajectory state value at the next moment for iteration to calculate the state prediction set at the next moment; (6) The optimal outer envelope set P corresponding to the output state set k (X k ), State estimation set X k State estimate x k and the area of ​​the set of states.

2. The surgical robot trajectory localization method based on parallelogram set member estimation according to claim 1, characterized in that, The NDI sensor error boundary condition in step (1) is: An observation equation y is constructed based on the relationship between the robot trajectory and the NDI sensor observations. k =Cx k +v k , where x k Let v be the state value at time k, C be the observation matrix, and v be the state value at time k. k Let be the NDI sensor noise at time k; where, This is the set value.

3. The surgical robot trajectory localization method based on parallelogram set member estimation according to claim 1, characterized in that, Step (2) specifically involves: The robot's state equations are constructed as follows: Where, x k-1 Let u be the robot's state value at time k-1. k-1 Let w be the trajectory interpolation value of the surgical robot at time k-1 in the trajectory estimation, and let w be a constant. k-1 The state noise at time k-1 satisfies the bounded condition. For x k-1 The predicted state estimate for the next time step. Here, A and B are set values, and they are equation coefficients. Then solve for the predicted estimate. The outer envelope of the parallelogram: In surgical robot trajectory planning, a position coordinate system is established on the trajectory osteotomy plane. The envelope prediction estimate is obtained by dynamically translating two non-parallel straight lines within the position coordinate system. Parallelogram P(θ,X0,E): In the formula, θ = (θ1, θ2), X0 = (x0, y0), E = (e1, e2); θ = (θ1, θ2) are the angles between the two non-parallel lines and the vertical axis of the coordinate system, (x, y) are the intersection points of the two non-parallel lines and the vertical axis, e1, e2 are the distances from the midline of the two non-parallel lines of parallelogram P(θ, X0, E), and (x0, y0) is the center point of parallelogram P(θ, X0, E), which is also the estimated state value predicted at the corresponding time. Therefore, all predicted estimates The set of predicted states formed The set of outer envelope parallelograms is represented as: In the formula, For the set of predicted states The set of distances from two sides to the median of all parallelograms whose outer envelope is in the given set. For the set of predicted states The set of outer envelope rotation angles of all outer envelope parallelograms in the set.

4. The surgical robot trajectory localization method based on parallelogram set member estimation according to claim 3, characterized in that, In step (3), the uniform state set represented by the observations is obtained by inverse solving using the observation model and observation information. and the corresponding outer envelope The specific process is as follows: The consistent state is obtained by solving the problem using the observation model and the position information collected by the NDI sensor. and consistent state set The parallelogram envelope method is used to describe the set of uniform states: In the formula, Let x′ be the set of uniform states with an irregular envelope. k It is the center of the set of parallelograms in a uniform state. The distance from the boundary of the uniform state set to the center line. C represents the parallelogram outer envelope parameter of the uniform state equation. k For the observation matrix, V represents the observation noise boundary. k This represents a set of noise.

5. The surgical robot trajectory localization method based on parallelogram set member estimation according to claim 4, characterized in that, The specific process of step (4) is as follows: Using the predicted state set and consistent state set The state truth information contained therein consists of the intersection set of all conditions satisfied after fusion, obtained from both the model and observation perspectives. The process of merging the intersection of the two sets satisfies the following equation: Where, x k Let k be the true value of the state at time k. Let x be the state truth value. k To solve for the truth estimation set at the next time step, the irregular envelope set needs to be enveloped into a parallelogram-shaped state estimation set. In the formula, Let be the predicted estimate set of the parallelogram envelope obtained from the state set at time k-1. Let P(conv(X) be the set of consistent states of the regular envelope. k )) is the set of state estimates for the parallelogram envelope; Predict the state set P(θ) for a parallelogram k ,x k ,e k and the set of consistent states Solving for the intersection yields the following fusion equation: In the formula, θ = (θ1, θ2) is the set of states P(θ). k ,x k ,e k ) parameter θ k (x, y) represents the state set parameters x. k λ represents the parameters of different area estimation sets that satisfy the fusion condition, and P λ (θ′ k ,x′ k ,e′ k ,λ) represents the parameterized state estimate after fusion, θ′ k ,x′ k ,e′ k Let represent the parameters of the parallelogram group that meets the conditions after the intersection is merged, where the truth value must be enveloped by the solution set in each set group; Solving the above intersection equation yields: In the solution of parameter θ, it is expressed as p(θ)=θ·I2, where I2 is a two-dimensional identity matrix.

6. The surgical robot trajectory localization method based on parallelogram set member estimation according to claim 5, characterized in that, In step (5), the solution set of the state estimation set is optimized to find the minimum area state estimation set, and the corresponding optimal outer envelope set is represented as P. k (X k Specifically: Where λ is the parameter of the estimated set solution group in step (4), and n represents the dimension of the state value.

Images