Inertia and optics combined tracking and positioning method and system based on ESKF algorithm

Abstract

The invention provides an inertial and optical combined tracking and positioning method and system based on an ESKF algorithm, which can be applied to the technical fields of optical positioning, inertial navigation tracking and combined navigation positioning and real-time navigation of surgical instruments. The method comprises the following steps: acquiring optical pose data of a surgical instrument through an optical positioning tracking module, and acquiring inertial pose data of the surgical instrument through an inertial measurement module; using an ESKF filter to predict a nominal state and an error state of the surgical instrument at each moment according to the inertial pose data, and updating the error state at each moment according to the optical pose data; according to the nominal state and the updated error state, the real state of the surgical instrument at each moment is determined, and the real state is used for indicating the real pose of the surgical instrument.

Description

Technical Field

[0001] The present disclosure relates to the technical fields of optical positioning, inertial navigation tracking, and combined navigation positioning and real-time navigation of surgical instruments, and more specifically, to an inertial and optical combined tracking and positioning method and system based on an ESKF algorithm. Background Art

[0002] With the rapid development of robotics in recent decades, robot-assisted systems have been widely used in many fields, such as the medical field. For example, robot-assisted systems in the medical field can assist surgical work through computers and robotics, such as robots specifically suitable for surgery.

[0003] At present, the navigation and tracking system of the robot-assisted system usually uses mechanical, electromagnetic, optical and other tracking methods for tracking and navigation, especially the optical tracking system (OTS). However, the OTS system has the problem of field of view obstruction in complex environments, which will cause the positioning and tracking process to be interrupted, affecting the accuracy and real-time positioning of the robot and surgical instruments.

[0004] In 2014, He Changyu, a scholar from Beijing Institute of Technology, proposed a sensor fusion method that uses an inertial measurement unit (IMU) to compensate for partial occlusion of optical tracking markers. In this system, the direction is obtained through inertial measurement, the inertial accelerometer and magnetometer measurement data are updated in real time, and the orientation measured by the optical camera estimates the deviation of the inertial sensor; in 2015, Zhao Peng from Tsinghua University studied the method of solving the target posture using inertial positioning information, designed experiments to verify the feasibility and accuracy of the inertial positioning system, and used the extended Kalman filter (EKF) to achieve the continuity and real-time positioning and tracking under short-term occlusion; in 2015, Nima Enayati and other scholars from the Polytechnic Institute of Milan, Italy, used a combined navigation system built with IMU and OTS to propose a method based on the Unscented Kalman Filter (UKF). A sensor fusion algorithm based on the Unified Kernel Filter (UKF) is used for robust estimation of the position and orientation of freely moving targets in surgical robotic applications. The orientation is represented by quaternions, which avoids singularities and is computationally more efficient.

[0005] However, the use of only a single tracking system limits the navigation performance of the robot to the inherent limitations of each tracking method. Existing combined navigation research is mostly in the fields of autonomous driving and satellite navigation, and is relatively less used in robot-assisted orthopedic surgical instruments. At the same time, the existing combined navigation data fusion algorithm is usually Kalman Filter (KF). KF is only applicable to linear systems with Gaussian white noise. For combined navigation systems used in surgical instruments, the environment is relatively complex, and the nonlinear equations need to be locally linearized to obtain new filtering equations. EKF is only applicable to first-order nonlinear systems, and the solution of the Jacobian matrix occupies a lot of computing resources, which is not conducive to real-time positioning and tracking; UKF needs to approximate the state vector, which increases the computational complexity, especially when processing high-dimensional data. The amount of calculation increases significantly, affecting the real-time performance of tracking and positioning. In addition, as direct filtering methods, KF and EKF are not suitable for complex nonlinear application scenarios. Summary of the invention

[0006] In view of this, the present disclosure provides an inertial and optical combined tracking and positioning method and system based on the ESKF algorithm.

[0007] One aspect of the present disclosure provides an inertial and optical combined tracking and positioning method based on an ESKF algorithm, including: acquiring optical posture data of a surgical instrument through an optical positioning tracking module, and acquiring inertial posture data of the surgical instrument through an inertial measurement module; using an ESKF filter to predict the nominal state and error state of the surgical instrument at each moment based on the inertial posture data, and updating the error state at each moment based on the optical posture data; determining the true state of the surgical instrument at each moment based on the nominal state and the updated error state, wherein the true state is used to indicate the true posture of the surgical instrument.

[0008] According to an embodiment of the present disclosure, the nominal state and error state of the surgical instrument at each moment are predicted based on the inertial posture data, including: predicting the nominal state at the kth moment based on the nominal state predicted at the k-1th moment and the inertial posture data measured at the k-1th moment, where k is an integer greater than 1; predicting the error state at the kth moment based on the inertial posture data measured at the k-1th moment and the error state measured at the k-1th moment.

[0009] According to an embodiment of the present disclosure, the error state at each moment is updated according to the optical posture data, including: determining the covariance matrix of the error state at the kth moment according to the disturbance noise covariance matrix, the disturbance noise Jacobian matrix, the Jacobian matrix of the error state system and the covariance matrix of the error state at the k-1th moment; determining the Kalman gain at the kth moment according to the measurement noise parameter, the observation Jacobian matrix of the error state at the kth moment, and the covariance matrix of the error state at the kth moment; substituting the predicted true state at the kth moment into the measurement function of the extended Kalman filter, and determining the updated error state at the kth moment according to the substituted value of the measurement function, the Kalman gain at the kth moment, and the optical posture data measured at the kth moment; updating the covariance matrix of the error state at the kth moment according to the Kalman gain at the kth moment, the observation Jacobian matrix of the error state at the kth moment, and the covariance matrix of the error state at the kth moment.

[0010] According to an embodiment of the present disclosure, the method also includes: based on the chain rule, according to the standard Jacobian matrix of the measurement parameters of the optical positioning tracking module and the Jacobian matrix of the error state, determining the observation Jacobian matrix of the error state at the kth moment.

[0011] According to an embodiment of the present disclosure, the true state of the surgical instrument at each moment is determined based on the nominal state and the updated error state, including: adding the nominal state at the kth moment and the updated error state to obtain the true state of the surgical instrument at the kth moment.

[0012] According to an embodiment of the present disclosure, the method also includes: after determining the true state of the surgical instrument at the kth moment, resetting the error state at the kth moment to zero, and resetting the covariance matrix of the updated error state at the kth moment using the partial derivative matrix of the reset function.

[0013] According to an embodiment of the present disclosure, the method also includes: after acquiring the inertial posture data of the surgical instrument, converting the inertial posture data based on the carrier coordinate system into inertial posture data in the navigation coordinate system where the optical posture data is located through the strapdown inertial navigation principle, so as to use the ESKF filter to predict the nominal state and error state of the surgical instrument at each moment based on the inertial posture data after the coordinate system is converted.

[0014] According to an embodiment of the present disclosure, the covariance matrix of the error state at the kth moment is determined by the following formula:

[0015]

[0016] in, represents the disturbance noise covariance matrix at the k-1th moment, represents the disturbance noise Jacobian matrix at the k-1th moment, represents the Jacobian matrix of the error state system at the k-1th moment, The covariance matrix representing the error state at the k-1th moment represents the covariance matrix of the error state at the kth moment, and T represents the transpose of the matrix.

[0017] According to an embodiment of the present disclosure, the Kalman gain at the kth moment is determined by the following formula:

[0018]

[0019] in, represents the measurement noise parameter, The observation Jacobian matrix representing the error state at the kth moment, represents the covariance matrix of the error state at the kth moment, represents the Kalman gain at the kth moment, and T represents the transpose of the matrix;

[0020] The error state after the k-th moment update is determined by the following formula:

[0021]

[0022] in, represents the measurement function of the extended Kalman filter for the real state, represents the true state of the prediction at the kth moment, Represents the optical posture data measured at the kth moment;

[0023] The covariance matrix of the error state at the kth moment after the update is determined by the following formula:

[0024]

[0025] in, represents the covariance matrix of the error state at the kth moment after the update, and I represents the identity matrix.

[0026] Another aspect of the present disclosure provides an inertial and optical combined tracking and positioning system based on the ESKF algorithm, including: an optical positioning tracking module, an inertial measurement module, and an inertial and optical combined tracking and positioning module based on the ESKF algorithm, wherein the optical positioning tracking module is configured to collect and acquire the optical posture data of the surgical instrument, and send the optical posture data to the inertial and optical combined tracking and positioning module based on the ESKF algorithm; the inertial measurement module is configured to collect and acquire the inertial posture data of the surgical instrument, and send the inertial posture data to the inertial and optical combined tracking and positioning module based on the ESKF algorithm; the inertial and optical combined tracking and positioning module based on the ESKF algorithm is configured to execute the above-mentioned inertial and optical combined tracking and positioning method based on the ESKF algorithm.

[0027] In the embodiments of the present disclosure, since the prediction of the error state and the nominal state is performed based on the inertial posture data, even if the optical positioning and tracking module cannot obtain the optical posture data of the surgical instrument due to the obstruction of the visual field, the real state can be obtained by superimposing the predicted error state and the nominal state, thereby avoiding the problem of positioning interruption caused by the obstruction of the visual field during the positioning and tracking of the surgical instrument, and improving the real-time tracking and positioning. In addition, since the error of the inertial posture data collected by the IMU will increase with time, the error state at each moment is updated according to the optical posture data, and the real state of the surgical instrument at each moment is determined according to the nominal state and the updated error state, thereby avoiding the accumulation of errors of the IMU, thereby improving the real-time and accuracy of tracking and positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] The above and other objects, features and advantages of the present disclosure will become more apparent through the following description of the embodiments of the present disclosure with reference to the accompanying drawings, in which:

[0029] Figure 1 The flowchart of the inertial and optical combined tracking and positioning method based on the ESKF algorithm to which the present disclosure can be applied is schematically shown.

[0030] Figure 2 The flowchart of determining the true state of the ESKF filter according to the embodiment of the present disclosure is schematically shown.

[0031] Figure 3 The flowchart of strapdown inertial navigation mechanical arrangement according to an embodiment of the present disclosure is schematically shown.

[0032] Figure 4 A scene diagram of inertial and optical combined tracking and positioning based on the ESKF algorithm according to an embodiment of the present disclosure is schematically shown.

[0033] Figure 5 The flowchart of combined navigation according to an embodiment of the present disclosure is schematically shown.

[0034] Figure 6 A block diagram of an inertial and optical combined tracking and positioning system based on an ESKF algorithm according to an embodiment of the present disclosure is schematically shown. DETAILED DESCRIPTION

[0035] Hereinafter, embodiments of the present disclosure will be described with reference to the accompanying drawings. However, it should be understood that these descriptions are exemplary only and are not intended to limit the scope of the present disclosure. In the following detailed description, for ease of explanation, many specific details are set forth to provide a comprehensive understanding of the embodiments of the present disclosure. However, it is apparent that one or more embodiments may also be implemented without these specific details. In addition, in the following description, descriptions of known structures and technologies are omitted to avoid unnecessary confusion of the concepts of the present disclosure.

[0036] The terms used herein are only for describing specific embodiments and are not intended to limit the present disclosure. The terms "comprise", "include", etc. used herein indicate the existence of the features, steps, operations and / or components, but do not exclude the existence or addition of one or more other features, steps, operations or components.

[0037] All terms (including technical and scientific terms) used herein have the meanings commonly understood by those skilled in the art unless otherwise defined. It should be noted that the terms used herein should be interpreted as having a meaning consistent with the context of this specification and should not be interpreted in an idealized or overly rigid manner.

[0038] When using expressions such as "at least one of A, B, and C, etc.", they should generally be interpreted according to the meaning of the expression commonly understood by those skilled in the art (for example, "a system having at least one of A, B, and C" should include but is not limited to a system having A alone, B alone, C alone, A and B, A and C, B and C, and / or A, B, C, etc.).

[0039] In order to solve the technical problem that the real-time and accuracy of positioning tracking in the OTS system is reduced due to occlusion of the tracking field of view, the embodiments of the present invention provide an inertial and optical combined tracking and positioning method and system based on the ESKF algorithm, which utilizes the combination of an inertial measurement module and an optical positioning tracking module and adopts an error state Kalman filter algorithm (Error State Kalman Filter, ESKF) to realize the determination of the true state of the surgical instrument.

[0040] Figure 1 The flowchart of the inertial and optical combined tracking and positioning method based on the ESKF algorithm to which the present disclosure can be applied is schematically shown.

[0041] like Figure 1 As shown, the method 100 includes operations S110 to S120.

[0042] In operation S110, the optical posture data of the surgical instrument is acquired through the optical positioning tracking module, and the inertial posture data of the surgical instrument is acquired through the inertial measurement module.

[0043] The surgical instrument can be fixed at the end of the robot arm, and the inertial measurement module can be an IMU, which is set on the surgical instrument to detect the inertial posture data of the surgical instrument in real time. The optical positioning tracking module, that is, the OTS system, can be set on the robot arm to calculate the optical posture data of the surgical instrument through the markers of the surgical instrument in the camera.

[0044] In one embodiment, the inertial measurement module may include an accelerometer and a gyroscope, and the inertial posture data may include accelerometer data and gyroscope data, such as three-axis acceleration, three-axis angular velocity, and quaternion representing the posture. The optical posture data may include three-axis position and quaternion representing the posture.

[0045] According to an embodiment of the present disclosure, in a combined navigation scenario, the optical posture data and inertial posture data of the optical positioning and tracking module can be unified in time by using the timestamp of the P2P Ethernet protocol and taking the timestamp corresponding to the inertial posture data output by the IMU as a reference.

[0046] According to an embodiment of the present disclosure, the inertial posture data of the inertial measurement module is for the carrier coordinate system, that is, the coordinate system where the robot arm is located, and the optical posture data of the optical positioning and tracking module is for the navigation coordinate system.

[0047] According to an embodiment of the present disclosure, in an integrated navigation scenario, before using the inertial pose data, the coordinate systems of the inertial pose data and the optical pose data may be unified so that the data in the same coordinate system can be used for integrated navigation later.

[0048] In operation S120, the ESKF filter is used to predict the nominal state and error state of the surgical instrument at each moment based on the inertial posture data, and the error state at each moment is updated based on the optical posture data; the actual state of the surgical instrument at each moment is determined based on the nominal state and the updated error state, wherein the actual state is used to indicate the actual posture of the surgical instrument.

[0049] According to an embodiment of the present disclosure, the ESKF filter refers to a filter that uses an Error State Kalman Filter (ESKF) algorithm.

[0050] According to the embodiments of the present disclosure, the true posture of the surgical instrument at each moment is regarded as two parts: the nominal part that is not affected by noise / disturbance and the noise / disturbance part, which are respectively called the nominal state and the error state. Therefore, the true state of the surgical instrument can be obtained by superimposing the nominal state and the error state.

[0051] The ESKF adopted in the implementation of the present disclosure is a typical indirect filtering method. The entire process is aimed at the error state of the system, that is, by predicting and updating the error state, the actual state obtained by superimposing the error state on the nominal state is more accurate.

[0052] The ESKF filter can be used to perform a prediction process and an update process, wherein the prediction process is used to predict the nominal state and error state of the surgical instrument at each moment based on the inertial posture data. Specifically, two parallel prediction processes 1 and 2 can be used to predict the nominal state and error state at the same moment, respectively. The update process is used to update the error state at each moment based on the optical posture data. After the update process, the true state of the surgical instrument at each moment can be obtained by superimposing the nominal state and the updated error state.

[0053] In the embodiments of the present disclosure, since the prediction of the error state and the nominal state is performed based on the inertial posture data, even if the optical positioning and tracking module cannot obtain the optical posture data of the surgical instrument due to the obstruction of the visual field, the real state can be obtained by superimposing the predicted error state and the nominal state, thereby avoiding the problem of positioning interruption caused by the obstruction of the visual field during the positioning and tracking of the surgical instrument, and improving the real-time tracking and positioning. In addition, since the error of the inertial posture data collected by the IMU will increase with time, the error state at each moment is updated according to the optical posture data, and the real state of the surgical instrument at each moment is determined according to the nominal state and the updated error state, thereby avoiding the accumulation of errors of the IMU, thereby improving the real-time and accuracy of tracking and positioning.

[0054] According to an embodiment of the present disclosure, the nominal state and error state of the surgical instrument at each moment are predicted based on the inertial posture data, including: predicting the nominal state at the kth moment based on the nominal state predicted at the k-1th moment and the inertial posture data measured at the k-1th moment, where k is an integer greater than 1; predicting the error state at the kth moment based on the inertial posture data measured at the k-1th moment and the error state measured at the k-1th moment.

[0055] According to an embodiment of the present disclosure, the nominal state predicted at the kth moment can be determined through the prediction process 1—nominal state prediction. The prediction process is shown in the following formula (1):

[0056] (1)

[0057] in, An iterative function representing the system state (including nominal state and error state), represents the estimated value of the nominal state at the kth moment, represents the predicted value of the nominal state at the k-1th moment, It represents the measurement value of the IMU at the k-1th moment, that is, the inertial position data at the k-1th moment.

[0058] Through prediction process 2—error state prediction, the predicted error state at the kth moment is determined. The prediction process is shown in the following formula (2):

[0059] (2)

[0060] in, represents the error state predicted at the kth moment, represents the true state measured at the k-1th moment, represents the error state measured at the k-1th moment, represents the Jacobian matrix of the error state system at the k-1th moment, represents the pulse of the constructed noise at the k-1th moment.

[0061] According to an embodiment of the present disclosure, the error state at each moment is updated according to the optical posture data, including: determining the covariance matrix of the error state at the kth moment according to the disturbance noise covariance matrix, the disturbance noise Jacobian matrix, the Jacobian matrix of the error state system and the covariance matrix of the error state at the k-1th moment; determining the Kalman gain at the kth moment according to the measurement noise parameter, the observation Jacobian matrix of the error state at the kth moment, and the covariance matrix of the error state at the kth moment; substituting the predicted true state at the kth moment into the measurement function of the extended Kalman filter, and determining the updated error state at the kth moment according to the substituted value of the measurement function, the Kalman gain at the kth moment, and the optical posture data measured at the kth moment; updating the covariance matrix of the error state at the kth moment according to the Kalman gain at the kth moment, the observation Jacobian matrix of the error state at the kth moment, and the covariance matrix of the error state at the kth moment.

[0062] According to an embodiment of the present disclosure, the covariance matrix of the error state at the kth moment is determined by the following formula:

[0063] (3)

[0064] in, represents the disturbance noise covariance matrix at the k-1th moment, represents the disturbance noise Jacobian matrix at the k-1th moment, represents the Jacobian matrix of the error state system at the k-1th moment, represents the covariance matrix of the error state at the k-1th moment, represents the covariance matrix of the error state at the kth moment, and T represents the transpose of the matrix.

[0065] According to an embodiment of the present disclosure, The Jacobian matrix of the error state system at the k-1th moment can be regarded as the transfer matrix of the entire system, and can be approximated to different accuracy levels in a variety of ways. In a specific embodiment, the system state function can be expressed in a simple Euler form. Taking the partial derivative of x at k-1 moments gives us:

[0066] also, The covariance of the preset noise of the speed, direction, acceleration and angular velocity over time may be integrated to obtain the obtained result. For example, the preset noise of the speed, direction (axis angle), acceleration and angular velocity may satisfy the Gaussian distribution.

[0067] According to an embodiment of the present disclosure, the Kalman gain at the kth moment is determined by the following formula:

[0068] (4)

[0069] in, represents the measurement noise parameter, The observation Jacobian matrix representing the error state at the kth moment, represents the covariance matrix of the error state at the kth moment, represents the Kalman gain at the kth moment, T represents the transpose of the matrix, and -1 represents the inverse of the matrix.

[0070] The error state after the k-th moment update is determined by the following formula:

[0071] (5)

[0072] in, represents the measurement function of the extended Kalman filter for the real state, represents the true state of the prediction at the kth moment, Represents the optical pose data measured at the kth moment.

[0073] The covariance matrix of the error state at the kth moment after the update is determined by the following formula:

[0074] (6)

[0075] in, represents the covariance matrix of the error state at the kth moment after the update, and I represents the identity matrix.

[0076] According to an embodiment of the present disclosure, the method also includes: based on the chain rule, according to the standard Jacobian matrix of the measurement parameters of the optical positioning tracking module and the Jacobian matrix of the error state, determining the observation Jacobian matrix of the error state at the kth moment.

[0077] In the embodiment of the present disclosure, the mean value of the error state is 0 during the observation process (such as setting the noise of the error state under Gaussian distribution). Therefore, there will be a real state x at a certain moment in the observation process, and the observed error state is 0. Therefore, x at this moment can be used as the nominal error, and the observed Jacobian matrix of the error state can be solved based on the chain rule. Specifically, the observed Jacobian matrix H of the error state can be calculated according to the following formula:

[0078] (7)

[0079] in, Represents the actual state at any moment.

[0080] The observed Jacobian matrix of the error state at the kth moment is calculated according to the following formula:

[0081] (8)

[0082] in, represents the standard Jacobian matrix of the measurement parameters for the optical positioning tracking module, and They represent the position and quaternion of the optical positioning tracking module at the kth moment, , , They represent the velocity, acceleration noise and angular velocity noise at the kth moment respectively.

[0083] The Jacobian matrix for the error state is calculated as follows:

[0084] (9)

[0085] in, , , , , Represent position, quaternion, velocity, acceleration noise and angular velocity noise respectively, , , , , They represent the error states of position, quaternion, velocity, acceleration noise, and angular velocity noise respectively.

[0086] According to an embodiment of the present disclosure, the true state of the surgical instrument at each moment is determined based on the nominal state and the updated error state, including: adding the nominal state at the kth moment and the updated error state to obtain the true state of the surgical instrument at the kth moment.

[0087] In the embodiment of the present disclosure, the nominal state at the kth moment calculated according to formula (1) and the updated error state at the kth moment calculated according to formula (5) can be added to obtain the true state of the surgical instrument at the kth moment. The specific calculation process is shown in the following formula (10):

[0088] (10)

[0089] in, represents the estimate of the true state of the system at the kth moment, represents the nominal state predicted at the kth moment, represents the error state after the update at the kth moment, Represents the summation under each state quantity.

[0090] According to the embodiments of the present disclosure, the actual state at the current time k can be estimated according to the above formulas (1) to (10), and the surgical instrument can be moved to the corresponding position of the actual state, thereby improving the accuracy of tracking and positioning.

[0091] To avoid the accumulation of IMU errors, after using the updated error state to calculate the true state at the current moment, the error state must be reset.

[0092] According to an embodiment of the present disclosure, after determining the true state of the surgical instrument at the kth moment, the error state at the kth moment is reset to zero, and the covariance matrix of the updated error state at the kth moment is reset using the partial derivative matrix of the reset function.

[0093] In this embodiment, the error state at the kth moment can be reset to zero and the covariance matrix of the error state can be reset based on the following formulas (11) and (12), respectively:

[0094] (11)

[0095] (12)

[0096] in, represents the covariance matrix of the error state after reset, represents the partial derivative matrix of the reset function at the kth moment. When calculating the covariance matrix of the error state at the k+1th moment according to formula (3), we can Can be regarded as calculate .

[0097] In an embodiment of the present disclosure, the partial derivative matrix G of the reset function can be calculated according to the following formula:

[0098] (13)

[0099] Where I is the identity matrix, I 6 is the 6-dimensional identity matrix, Represents the shaft angle.

[0100] For ease of understanding, the following will be Figure 2 Let's take this as an example to explain.

[0101] Figure 2 The flowchart of determining the true state of the ESKF filter according to the embodiment of the present disclosure is schematically shown.

[0102] like Figure 2 As shown, prediction process 1 and prediction process 2 are performed using the inertial posture data obtained by the IMU. Prediction process 1 is implemented by formula (1), and prediction process 2 is implemented by formulas (2) and (3). Afterwards, the optical posture data measured by the OTS is used to update the error state and the covariance matrix of the error state calculated by the prediction process at the kth moment, through the calibration process. The calibration process can be implemented by formulas (4) to (6). According to the error state at the kth moment updated by the calibration process and the nominal state at the kth moment predicted by prediction process 1, the actual state at the kth moment is calculated. The calculation process is shown in formula (10). Then, the error state and the covariance matrix of the error state at the kth moment are reset through the reset process. The reset operation is shown in formulas (11) and (12).

[0103] According to an embodiment of the present disclosure, after acquiring the inertial posture data of the surgical instrument, the inertial posture data based on the carrier coordinate system is converted into inertial posture data in the navigation coordinate system where the optical posture data is located through the strapdown inertial navigation principle, so as to use the ESKF filter to predict the nominal state and error state of the surgical instrument at each moment based on the inertial posture data after the coordinate system is converted.

[0104] Figure 3 The flowchart of strapdown inertial navigation mechanical arrangement according to an embodiment of the present disclosure is schematically shown.

[0105] like Figure 3As shown, the three-axis specific force f and three-axis angular velocity can be collected by the accelerometer and gyroscope of the IMU Using the initial posture data Att 0 and the three-axis angular velocity Perform attitude calculation to obtain the quaternion of the attitude; then based on the principle of strapdown inertial navigation, according to the three-axis specific force f and the result of attitude calculation, convert the initial inertial posture data in the navigation coordinate system, and then correct the initial velocity v 0 , initial position P 0 Solve for the corresponding position and velocity.

[0106] Figure 4 A scene diagram of inertial and optical combined tracking and positioning based on the ESKF algorithm according to an embodiment of the present disclosure is schematically shown.

[0107] According to an embodiment of the present disclosure, the scene diagram of the combined tracking and positioning of the integrated IMU and OTS is as follows: Figure 4 As shown. It mainly includes three processes: data analysis, prediction process and update process, which together constitute the ESKF positioning algorithm of IMU-OTS. In the ESKF principle, it is assumed that noise and disturbance are both Gaussian distributed random processes with variance and mean. At the same time, ESKF performs a filtered estimate on the error state and substitutes the filtered error state value into the nominal state. In this process, the error state quantity and the nominal state quantity of ESKF are iterated simultaneously, and the error state will be reset after observation correction to ensure the transmission of variance.

[0108] During the data analysis process, IMU data can be read, mainly including accelerometer data and gyroscope data. Through mechanical arrangement, the strapdown inertial navigation system can convert the inertial posture data output by the IMU into position, velocity and attitude information through numerical calculation, that is, the conversion of the coordinate system. The inertial posture data after the coordinate system conversion is used for nominal state prediction and error state prediction respectively. In the prediction process, after the error state prediction, the covariance matrix of the error state can be determined by formula (3). After that, after the OTS data is read and the gain matrix of the Kalman gain is calculated, the error state can be updated through the state calibration operation. After that, the updated error state can be substituted into formula (10) and summed with the nominal state to obtain the true state. After determining the true state, the error reset is used to obtain a new error state with a covariance sum of 0. In summary, the iteration at the current moment is completed; then, the next round of iteration is carried out with the help of the data after the error reset.

[0109] In the embodiments of the present disclosure, the use of ESKF has the following significant advantages over other filtering algorithms: 1) Since it has the same number of parameters as the degrees of freedom, the directional error state is minimal, avoiding problems related to parameter redundancy and the singularity of the covariance matrix. 2) By resetting the error, the error state system is always located near the origin and away from the parameter singularity point, thereby ensuring that the effectiveness of the linearization remains unchanged. 3) The error state system has a small value, and all its second-order terms can be ignored, which facilitates efficient and fast calculation of the Jacobian matrix. 4) The error state changes slowly. When individual signals are not ideal, the absolute estimate of the positioning will not be immediately deteriorated, and the system has better adaptability and stronger robustness.

[0110] Figure 5 The flowchart of combined navigation according to an embodiment of the present disclosure is schematically shown.

[0111] like Figure 5 As shown, under inertial navigation, inertial posture data can be obtained through the inertial measurement module; under optical navigation, optical posture data can be obtained through the optical positioning tracking module. After the coordinate systems of the inertial posture data and the optical posture data are unified, the ESKF filter can be used to calculate the true posture. In the case of field obstruction in optical navigation, positioning can be performed through inertial navigation, such as summing the nominal state and error state calculated by the inertial posture data to obtain the true state; in the case of field obstruction in optical navigation, positioning can be performed through combined navigation (inertial navigation + optical navigation), such as calculating the nominal state and error state using the inertial posture data, and then updating the error state using the optical posture data, and summing the nominal state and the updated error state to obtain the true state; in the case of no field obstruction in optical navigation, positioning can be performed through separate optical navigation to ensure the positioning accuracy of the true posture of the surgical instrument when there is no field obstruction.

[0112] Therefore, when the optical marker is observable, the embodiments of the present disclosure can improve the accuracy of the posture and position accuracy based on OTS, and the effect is better than the traditional linear filtering algorithm of KF and other nonlinear filtering algorithms such as EKF and UKF. When the optical marker is blocked, ESKF can fill the gap of OTS posture estimation when the line of sight is blocked.

[0113] Figure 6 A block diagram of an inertial and optical combined tracking and positioning system based on an ESKF algorithm according to an embodiment of the present disclosure is schematically shown.

[0114] According to an embodiment of the present disclosure, an inertial and optical combined tracking and positioning system 600 based on an ESKF algorithm includes an optical positioning tracking module 610 , an inertial measurement module 620 , and an inertial and optical combined tracking and positioning module 630 based on an ESKF algorithm.

[0115] The optical positioning tracking module 610 is configured to collect and acquire the optical posture data of the surgical instrument, and send the optical posture data to the inertial and optical combined tracking and positioning module based on the ESKF algorithm;

[0116] The inertial measurement module 620 is configured to collect and acquire the inertial posture data of the surgical instrument, and send the inertial posture data to the inertial and optical combined tracking and positioning module based on the ESKF algorithm;

[0117] The inertial and optical combined tracking and positioning module 630 based on the ESKF algorithm is configured as follows: obtaining the optical posture data of the surgical instrument through the optical positioning tracking module and obtaining the inertial posture data of the surgical instrument through the inertial measurement module; using the ESKF filter to predict the nominal state and error state of the surgical instrument at each moment according to the inertial posture data, and updating the error state at each moment according to the optical posture data; determining the actual state of the surgical instrument at each moment according to the nominal state and the updated error state, wherein the actual state is used to indicate the posture of the surgical instrument.

[0118] Specifically, the inertial and optical combined tracking and positioning module 630 based on the ESKF algorithm is configured to perform operations that are the same as or similar to the inertial and optical combined tracking and positioning method based on the ESKF algorithm, which will not be described in detail herein.

[0119] The flowcharts and block diagrams in the accompanying drawings illustrate the possible implementation architecture, functions and operations of the systems, methods and computer program products according to various embodiments of the present disclosure. In this regard, each box in the flowchart or block diagram may represent a module, a program segment, or a part of a code, and the above-mentioned module, program segment, or a part of the code contains one or more executable instructions for implementing the specified logical function. It should also be noted that in some alternative implementations, the functions marked in the box may also occur in an order different from that marked in the accompanying drawings. For example, two boxes represented in succession can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each box in the block diagram or flowchart, and the combination of boxes in the block diagram or flowchart, can be implemented with a dedicated hardware-based system that performs a specified function or operation, or can be implemented with a combination of dedicated hardware and computer instructions. It can be understood by those skilled in the art that the features recorded in the various embodiments of the present disclosure can be combined and / or combined in a variety of ways, even if such a combination or combination is not explicitly recorded in the present disclosure. In particular, without departing from the spirit and teaching of the present disclosure, the features described in the various embodiments of the present disclosure may be combined and / or combined in a variety of ways. All of these combinations and / or combinations fall within the scope of the present disclosure.

[0120] The embodiments of the present disclosure are described above. However, these embodiments are only for illustrative purposes and are not intended to limit the scope of the present disclosure. Although the embodiments are described above, this does not mean that the measures in the various embodiments cannot be used in combination to advantage. Without departing from the scope of the present disclosure, those skilled in the art may make a variety of substitutions and modifications, which should all fall within the scope of the present disclosure.

Claims

1. An inertial and optical combined tracking and positioning method based on ESKF algorithm, characterized in that: The method comprises: Acquire optical posture data of the surgical instrument through an optical positioning tracking module, and acquire inertial posture data of the surgical instrument through an inertial measurement module; Using the ESKF filter, the nominal state and error state of the surgical instrument at each moment are predicted according to the inertial posture data, and the error state at each moment is updated according to the optical posture data; the real state of the surgical instrument at each moment is determined according to the nominal state and the updated error state, wherein the real state is used to indicate the real posture of the surgical instrument.

2. The method according to claim 1, characterized in that The predicting the nominal state and error state of the surgical instrument at each moment according to the inertial posture data includes: Predict the nominal state at the kth moment according to the nominal state predicted at the k-1th moment and the inertial posture data measured at the k-1th moment, where k is an integer greater than 1; According to the inertial posture data measured at the k-1th moment and the error state measured at the k-1th moment, the error state at the k-1th moment is predicted.

3. The method according to claim 1, characterized in that The updating of the error state at each moment according to the optical posture data comprises: Determine the covariance matrix of the error state at the kth moment according to the disturbance noise covariance matrix at the k-1th moment, the disturbance noise Jacobian matrix, the Jacobian matrix of the error state system, and the covariance matrix of the error state; Determine the Kalman gain at the kth moment according to the measurement noise parameter, the observation Jacobian matrix of the error state at the kth moment, and the covariance matrix of the error state at the kth moment; Substituting the predicted true state at the kth moment into the measurement function of the extended Kalman filter, and determining the updated error state at the kth moment according to the substitution value substituted into the measurement function, the Kalman gain at the kth moment, and the optical attitude data measured at the kth moment; The covariance matrix of the error state at the kth moment is updated according to the Kalman gain at the kth moment, the observation Jacobian matrix of the error state at the kth moment, and the covariance matrix of the error state at the kth moment.

4. The method according to claim 3, characterized in that The method further comprises: Based on the chain rule, the observation Jacobian matrix of the error state at the kth moment is determined according to the standard Jacobian matrix of the measurement parameters of the optical positioning tracking module and the Jacobian matrix of the error state.

5. The method according to claim 1, characterized in that Determining the true state of the surgical instrument at each moment according to the nominal state and the updated error state includes: The nominal state at the kth moment is added to the updated error state to obtain the true state of the surgical instrument at the kth moment.

6. The method according to claim 5, characterized in that The method further comprises: After determining the true state of the surgical instrument at the kth moment, the error state at the kth moment is reset to zero, and the covariance matrix of the updated error state at the kth moment is reset using the partial derivative matrix of the reset function.

7. The method according to any one of claims 1 to 6, characterized in that: The method further comprises: After acquiring the inertial posture data of the surgical instrument, the inertial posture data based on the carrier coordinate system is converted into inertial posture data in the navigation coordinate system where the optical posture data is located through the strapdown inertial navigation principle, so as to use the ESKF filter to predict the nominal state and error state of the surgical instrument at each moment according to the inertial posture data after the coordinate system is converted.

8. The method according to claim 3, characterized in that The covariance matrix of the error state at the kth moment is determined by the following formula: in, represents the disturbance noise covariance matrix at the k-1th moment, represents the disturbance noise Jacobian matrix at the k-1th moment, represents the Jacobian matrix of the error state system at the k-1th moment, The covariance matrix representing the error state at the k-1th moment represents the covariance matrix of the error state at the kth moment, and T represents the transpose of the matrix.

9. The method according to claim 3, characterized in that: The Kalman gain at the kth moment is determined by the following formula: in, represents the measurement noise parameter, The observation Jacobian matrix representing the error state at the kth moment, represents the covariance matrix of the error state at the kth moment, represents the Kalman gain at the kth moment, and T represents the transpose of the matrix; The error state after the k-th moment update is determined by the following formula: in, represents the measurement function of the extended Kalman filter for the real state, represents the true state of the prediction at the kth moment, Represents the optical posture data measured at the kth moment; The covariance matrix of the error state at the kth moment after the update is determined by the following formula: in, represents the covariance matrix of the error state at the kth moment after the update, and I represents the identity matrix.

10. An inertial and optical combined tracking and positioning system based on ESKF algorithm, characterized in that: The system includes: an optical positioning tracking module, an inertial measurement module, and an inertial and optical combined tracking and positioning module based on the ESKF algorithm. The optical positioning tracking module is configured to collect and acquire optical posture data of the surgical instrument, and send the optical posture data to the inertial and optical combined tracking and positioning module based on the ESKF algorithm; The inertial measurement module is configured to collect and acquire inertial posture data of the surgical instrument, and send the inertial posture data to the inertial and optical combined tracking and positioning module based on the ESKF algorithm; The inertial and optical combined tracking and positioning module based on the ESKF algorithm is configured to execute the method as described in any one of claims 1 to 9.

Images