Patent Grant
11883119
An embodiment of the invention relates to digital signal processing techniques for correcting a sensor circuit-generated signal that tracks the orientation or position of a handheld, robotic surgery user interface device. Other embodiments are also described.
In the field of robotic surgery, the movements of a user interface device (UID) such as a handheld manipulator (held in the surgeon's hand) are translated into corresponding movements of a joint on a robotic or manipulator arm to which a surgical tool is attached (also referred to as end effector.) This may give the surgeon greater visualization, enhanced dexterity, and greater precision, which benefit the patient in many ways including shorter hospitalization, faster recovery, and smaller incisions. A digital stream of sensed orientation data and position data, that may be time-stamped or that may be updated regularly according to a clock signal, is produced by a sensor subsystem. The stream represents real-time tracking of the UIDs orientation and position, also referred to as robotic surgery UID tracking (orientation and position) data. A part or all of the sensor subsystem may be inside the housing of the UID, e.g., using the raw output from inertial sensors such as an accelerometer and a gyro that are inside the UID housing. The UID tracking data may also be produced by a sensor subsystem such as a six-degree of freedom (6DOF) electromagnetic tracker that is suitable for medical or surgical applications, or by an optical tracker that is “looking” at markers on the UID. The tracking data may be relative (e.g., incremental displacement or orientation changes) or it may be absolute.
The tracking data is an input to a robotic surgery controller that translates the tracking data, which is a direct measure of the movement of the UID, into corresponding actuator commands for an end effector, so that the end effector mimics the movements of the UID (that are caused by for example the surgeon's hand, wrist and fingers.) As a practical matter, however, the tracking data (tracking input) is noisy or imprecise, and therefore needs to be corrected in order to ensure precision and repeatability in the movement of the end effector. In addition, while the tracking input may be time-stamped or it may be isochronous, its constituent measured values (measured samples) might have been produced or delivered by the sensor circuit at a different rate than desired by the robotic surgery controller that is consuming the tracking input, e.g., the sensor circuit sampling rate may be slower than the sample rate of a discrete time domain (digital) controller.
An embodiment of the invention is a linear quadratic estimator, LQE, that corrects a noisy or imprecise robotic surgery UID tracking input, and may also inherently up-sample the tracking input to come closer to or match the higher input sample rate of a robotic surgery control system controller, and may do so without introducing an unduly high latency. In one embodiment, the LQE (or corrector) integrates the latency reduction and up-sampling functions into its prediction phase that is repeating at a higher rate than the UID tracking input to produce output samples, and switches to its update phase when a new tracking input sample is received. In the update phase, a Kalman filter gain may be computed that is used to compute an updated output sample of the LQE. In one embodiment, a latency coefficient used in the prediction phase has been tuned in advance, so that the LQE output exhibits reduced latency.
In another embodiment, a robotic surgery system has a processor and a machine-readable medium having stored therein instructions that when executed by the processor correct a UID tracking input, by producing an output sequence that is an estimate of a position or an orientation of the UID. The processor receives an input sequence that are measures of the position or the orientation of the UID, and computes a current output sample of the output sequence, wherein the current output sample is computed based on i) a previously computed output sample, and ii) a velocity term. When a new tracking input sample of the input sequence is received, a new output sample of the output sequence is computed, based on i) a previously computed output sample, and ii) the new tracking input sample. In one instance, the instructions are such that while the new tracking input sample has not been received, the current output sample in the output sequence is computed from a window of several previously computed output samples, by a) fitting a line to the previously computed output samples in the window, and b) taking a slope of the line to be the velocity term.
The above summary does not include an exhaustive list of all aspects of the present invention. It is contemplated that the invention includes all systems and methods that can be practiced from all suitable combinations of the various aspects summarized above, as well as those disclosed in the Detailed Description below and particularly pointed out in the claims filed with the application. Such combinations have particular advantages not specifically recited in the above summary.
The embodiments of the invention are illustrated by way of example and not by way of limitation in the figures of the accompanying drawings in which like references indicate similar elements. It should be noted that references to “an” or “one” embodiment of the invention in this disclosure are not necessarily to the same embodiment, and they mean at least one. Also, in the interest of conciseness and reducing the total number of figures, a given figure may be used to illustrate the features of more than one embodiment of the invention, and not all elements in the figure may be required for a given embodiment.
Several embodiments of the invention with reference to the appended drawings are now explained. Whenever the shapes, relative positions and other aspects of the parts described in the embodiments are not explicitly defined, the scope of the invention is not limited only to the parts shown, which are meant merely for the purpose of illustration. Also, while numerous details are set forth, it is understood that some embodiments of the invention may be practiced without these details. In other instances, well-known circuits, structures, and techniques have not been shown in detail so as not to obscure the understanding of this description.
A conventional linear quadratic estimator, LQE, has a prediction phase in which a system model is used to compute estimates of an output state variable (also referred to here as “output”), and an update phase in which the outcome of the present input measurement (which is of course corrupted by noise or imprecision) is used to update the estimates so that they tend to be more precise than the input measurements. The LQEs algorithm is recursive and can run in real time, using only the present input measurements and the previously calculated output (and its uncertainty matrix.)
An embodiment of the invention is a modified LQE or corrector that is particularly suitable for correcting the position, orientation or both of a robotic surgery UID, as represented in a tracking input sequence. The tracking input sequence may also be referred to here as a tracking input, or input measurement, and may be that of several position variables, several orientation variables, or both.
As a practical matter, the tracking input produced by the UID tracking subsystem 5 is noisy or imprecise, and therefore needs to be corrected in order to ensure precision and repeatability in the movement of the end effector 3. In addition, while the tracking input may be time-stamped or it may be isochronous, its constituent measured values (measured samples) may have been produced or delivered by the sensor circuit (in the subsystem 5) at a different rate than desired by the controller 8 that is consuming the tracking input, e.g., the sensor circuit sampling rate may be slower than the sample rate of a discrete time domain (digital) controller. A linear quadratic estimator, LQE 6, is thus provided to correct the tracking input.
In one embodiment, the LQE 6 may be described as having a prediction phase and an update phase, which together recursively compute the samples of an LQE state variable (output), such as position, in accordance with the flow diagram of
Also note that prior to sample 1, there may be an initialization stage in which the initial output state value (sample 1) is computed based on an input state (the tracking input or a measurement) or other initial condition, following which the next output state variable (sample 2 and subsequent samples) are computed recursively based on a previously computed output state value.
The prediction phase of the LQE 6 may be illustrated using the following example of a constant velocity mathematical system model, for position variable X. At a given point in time, t, the position of a particle may be represented by a continuous time system model,
X=X0+V0*t+½*A*t2 (1)
In one embodiment, the state variable X is a vector, X=[x, y, z, q0, q1, q2, q3] of Cartesian coordinates (x, y, z) as position data, and the quaternion (q0, q1, q2, q3) as orientation data. Note however that the LQE 6 may alternatively be applied to correct just the orientation data, while a non-estimating type low pass filter, e.g., a digital finite impulse response, FIR, filter, is applied to reduce noise in the position data only.
When the LQE 6 is implemented using a digital computer, a discrete time system model can be written as
X[n+1]=X[n]+V*delta_t+noise (2)
This equation may be used to compute the state variable X at time index n+1 (also referred to as a current output sample X[n+1]). That computation is based on i) a previously computed output sample X[n], ii) a computed velocity term V, and iii) an inter-sample time interval delta_t (that as described below has been adjusted by a tuning parameter, alpha.) Note that delta_t may refer to the time interval used in the system model to compute a current sample at index n+1 from a previous sample at index n, in the prediction phase (it is not the tracking input's sampling period, which is lower than the rate at which predictions are computed in the prediction phase.) It can be seen from Eq. 2 above that in addition to the previous estimate of the state X[n], the computation of X[n+1] relies on two primary terms, namely delta_t and a computation of the velocity term V both of which are described below in turn. Also, the velocity term V here is not a “state” of the LQE as is X, but rather an input or external term. As to the noise term, it may be assumed to be Gaussian noise with zero mean and a sigma standard deviation.
The update phase may use any conventional measurement model (that is applicable to the UID and its tracking data) to compute an updated output, Xupd (an “update” data point in
Xmeas=X[n+1]+noise (3)
where Xmeas is the position (x-y-z) measurement or tracking input and X[n+1] is the predicted state calculated in Eq. 2. The measurement noise term may be modeled by collecting UID position data (tracking input) at different positions and orientations of the UID 4 and then by calculating its histogram.
The update phase may proceed as follows: compare Xmeas with Xpred (e.g., a previously computed output, X[n+1]), and use a measure of that comparison to compute a Kalman gain, K, using a particular measurement model; if the difference between Xmeas and Xpred is large, then K becomes larger, and if the difference is small then K becomes smaller. The gain K is then used to compute the updated position Xupd, as
Xupd=Xpred+K*( . . . ) (4)
where known techniques can be used to compute the K*( . . . ) term. Xupd may then be taken as the updated, current output sample of the LQE (in this example, X[n+1].) The modified LQE may then proceed with computing the next output sample, here X[n+2], using its prediction phase operations as described above, and until another new tracking input is received that triggers the update phase.
From the above description, it is clear that the up-sampling of the tracking input by the modified LQE happens inherently when repeating the prediction phase at a higher rate than the tracking input sampling. Computation of X as the output state repeats until a new (e.g., a most recent) tracking input sample Xmeas arrives—see
Note that a further advantage of the above process is that the prediction and update phases may occur asynchronously, which allows up sampling to be performed as needed to fill in the “gap” between adjacent tracking input samples. For instance, in
One problem that arises is how to reduce latency of the LQE 6. In accordance with an embodiment of the invention, a tuning parameter, also referred to here as a latency coefficient, alpha, is defined such that a sum of alpha and a nominal output sampling period nominal_period for the LQE's output sequence, replaces delta_t in the discrete time system model equation above, as
delta_t=nominal_period+alpha (5)
Note that nominal_period is the nominal LQE output sampling period and is the same (constant) for a batch of UIDs and their respective tracking input correctors, while alpha is a tuning parameter that varies from specimen to specimen of that batch. For example, the nominal_period=1 msec, and alpha is a fraction of the nominal_period but may vary depending on the application (the particular UID and its sensor-generated tracking input.)
The latency coefficient alpha should be adjusted during a “calibration process”, e.g., at the factory, for each production UID specimen, until the LQE output (which is being computed in accordance with the prediction and update phases as described further below) properly predicts “what is going to happen”, by effectively shifting the LQE output sequence earlier in time, closer to the tracking input. This is illustrated in
While the addition of (and the tuning of) alpha causes a reduction in the effective latency of the LQE 6 coupled with improved noise reduction in the output state, this comes at the cost of (or in exchange for) the LQE output exhibiting overshoot when the tracking input changes direction. There is a need to recover quickly from this overshoot; the LQE 6 performs this recovery in its update phase.
Another problem that arises in the LQE prediction phase is how to calculate V to avoid taking the derivative of the known but noisy tracking input, X_meas. Proper computation of V helps further reduce noise at the output of the LQE 6, and has been shown to influence the choice of the tuning parameter alpha. In one embodiment of the invention, referring now to
In the example shown in
X(t)=A*t+B (6)
Note that the position data may include x, y and z values, such that A and B may be 3×1 vectors representing the polynomial coefficients for the three direction, x, y and z. The derivative of Eq. 5 is taken with respect to time to calculate the linear velocities V (in the x-y-z directions) as
dX(t)/dt=V(t)=A (7)
Note that the length k of the window (the number of samples in it) may be viewed as a second calibration parameter that can be used when tuning the latency coefficient alpha (see above.) Also, the window of length k moves or slides as shown in
Also, the system model used in the prediction phase need not change and may remain the same for every prediction, while the window of state values that are used to parameterize the position state variable (and compute the velocity) does move in time and so changes with each new prediction.
The description above used position only, as the tracking input, to illustrate the various concepts in an easier to understand manner. The concepts described above however are also applicable to and in fact may be more relevant when applied to orientation data in the tracking input. In that case, the orientation states, quaternions (q0, q1, q2, q3) may be assumed to evolve with a constant angular velocity model as seen in the equation below:
q[n+1]=exp(½*skew(w*(nominal_period+latency_parameter)))*q[n]+noise (8)
where q is a 4×1 quaternion vector, w is a 3×1 angular velocity vector around x, y, or z axis, and is used as an input term not as a state variable, skew( ) is the skew symmetric matrix operation, nominal_period is the nominal LQE output sampling period as described above in connection with Eq. 5, and latency_parameter is a tuning parameter (like alpha above) to account for latency. The noise term may again be assumed to be Gaussian noise with zero mean and sigma standard deviation.
Similar to the position estimation case illustrated above, in order to have smooth angular velocity estimates, the quaternion estimates are parameterized with respect to time and their derivatives are taken. To do this one case use the slerp equation
q(q0,q1,t)=q0*sin[(1−t)Ω]/sin(Ω)+q1*sin[tΩ]/sin(Ω) (9)
where q0 and q1 are 4×1 quaternion vectors corresponding to the estimated quaternions, window_length outputs before and the last output, respectively, as given by
q0=q[n−window_length]q1=q[n] (10)
Additionally in Eq. 9, Ω corresponds to the angle between q1 and q2, and finally t is the time variable.
In order to obtain a continuous function of the angular velocity with respect to time, Eq. 9 is differentiated to obtain q_dot
q_dot=dq/dt (11)
Next Eq. 11 is used to find the angular orientation using
w=2*transpose(Q(q[n]))*q_dot (12)
where Q is a 4×3 matrix formed by the latest quaternion estimate q[n] and it is calculated as
In the update phase, the UID tracking input is used to update the quaternion orientation estimates. The measurement model used in the update phase may be
q_uid=q[n+1]+noise (14)
where q_uid is the quaternion measurement (UID tracking input) and q[n+1] is the predicted state calculated in Eq. 8. The measurement noise term may be modeled similar to the position case.
Referring now to
While elements of position such as (x, y, z) coordinates are orthogonal or independent from each other in the sense that each could be filtered independently of the others and using a non-estimating, low pass filter, orientation elements such as (pitch, yaw, roll) or quaternion (3 angles, and 1 length) are not all independent from each other and as such need an estimating-type filter like the LQE 6 described above for the orientation case.
As explained above, an embodiment of the invention may be an article of manufacture in which a machine-readable medium (such as microelectronic memory) has stored therein instructions which program one or more data processing components (generically referred to here as “a processor” that executes the instructions) to perform the digital signal processing operations of the LQE 6 described above for correcting the UID tracking input. In other embodiments, some of these operations might be performed by specific hardware components that contain hardwired logic (e.g., dedicated digital filter blocks and state machines). Those operations might alternatively be performed by any combination of programmed data processing components and fixed hardwired circuit components.
While certain embodiments have been described and shown in the accompanying drawings, it is to be understood that such embodiments are merely illustrative of and not restrictive on the broad invention, and that the invention is not limited to the specific constructions and arrangements shown and described, since various other modifications may occur to those of ordinary skill in the art. For example, while a robotic surgery UID is held in a hand of a user, the tracking input correction techniques described above are also applicable to other applications including where the UID is attached to a head or to another limb of a user such as a leg or foot. The description is thus to be regarded as illustrative instead of limiting.
This patent application is continuation of U.S. Ser. No. 17/110,928 filed 3 Dec. 2020 which is a divisional of U.S. Ser. No. 15/900,552 filed Feb. 20, 2018.
| Number | Name | Date | Kind |
|---|---|---|---|
| 4442404 | Bergmann | Apr 1984 | A |
| 7453227 | Prisco | Nov 2008 | B2 |
| 8073528 | Zhao | Dec 2011 | B2 |
| 8079950 | Stern | Dec 2011 | B2 |
| 8287522 | Moses et al. | Oct 2012 | B2 |
| 8613724 | Lanier, Jr. et al. | Dec 2013 | B2 |
| 8996169 | Lightcap | Mar 2015 | B2 |
| 9220917 | Boyden et al. | Dec 2015 | B2 |
| 9296104 | Swarup et al. | Mar 2016 | B2 |
| 9727963 | Mintz | Aug 2017 | B2 |
| 9861447 | Hourtash | Jan 2018 | B2 |
| 11071441 | Yu | Jul 2021 | B2 |
| 11076918 | Quaid, III | Aug 2021 | B2 |
| 11413101 | Sen | Aug 2022 | B2 |
| 20060142657 | Quaid et al. | Jun 2006 | A1 |
| 20060258938 | Hoffman et al. | Nov 2006 | A1 |
| 20070252550 | Takaishi | Nov 2007 | A1 |
| 20120041263 | Sholev | Feb 2012 | A1 |
| 20130064427 | Picard | Mar 2013 | A1 |
| 20130218340 | Hager | Aug 2013 | A1 |
| 20140249654 | Blevins et al. | Sep 2014 | A1 |
| 20150018841 | Seo | Jan 2015 | A1 |
| 20150164609 | Wu | Jun 2015 | A1 |
| 20160025756 | Pollack et al. | Jan 2016 | A1 |
| 20170036346 | Kamioka | Feb 2017 | A1 |
| 20170084027 | Mintz | Mar 2017 | A1 |
| 20170252111 | Wu | Sep 2017 | A1 |
| 20180232048 | Popovich et al. | Aug 2018 | A1 |
| 20190041634 | Popovich et al. | Feb 2019 | A1 |
| 20190095644 | Park et al. | Mar 2019 | A1 |
| 20190380794 | Al Jewad et al. | Dec 2019 | A1 |
| Number | Date | Country |
|---|---|---|
| 2006091494 | Aug 2006 | WO |
| 2007136768 | Nov 2007 | WO |
| Entry |
|---|
| International Preliminary Report on Patentability for International Application No. PCT/US2018/019774 dated Sep. 3, 2020, 6 pages. |
| Wang, Yuguan, et al., “Filling the Gap between Low Frequency Measurements with Their Estimates,” Robotics and Automation (ICRA), 2014 IEEE International Conference on, May 31, 2014, 6 pages. |
| Eom, Ki Hwan, “Improved Kalman Filter Method for Measurement Noise Reduction in Multi Sensor RFID Systems,” Sensors, vol. 11, Oct. 28, 2011, pp. 10266-10282. |
| Tully, Stephen, et al., “Inequality Constrained Kalman Filtering for the Localization and Registration of a Surgical Robot,” Intelligent Robots and Systems (IROS), 2011 IEEE/RSJ International Conference on, Sep. 1, 2011, 7 pages. |
| Negenborn, Rudy, “Robot Localization and Kalman Filters: On Finding your position in a noisy world,” A thesis submitted to the Institute of Information and Computing Sciences in partial fulfillment of the requirements for the degree of Master of Science, specialized in Intelligent Systems, Sep. 1, 2003, 156 pages. |
| Kalman, R.E., “A New Approach to Linear Filtering and Predication Problems,” Transactions of the ASME—Journal of Basic Engineering, 82(Series D), Jan. 1, 1960, 12 pages. |
| International Search Report and Written Opinion for International Application No. PCT/US2018/019774 dated Nov. 13, 2018, 10 pages. |
| Ramadurai, Srikrishnan, et al., “Application of Unscented Kalman Filter to a Cable Driven Surgical Robot: A Simulation Study,” 2012 IEEE International Conference on Robotics and Automation, May 14, 2012, pp. 1495-1500. |
| Aravind, M.A., et al. “Optimal Position Control of a DC Motor Using LQG with EKF,” 2017 International Conference on Mechanical, System and Control Engineering, 2017, pp. 149-154. |
| Gudpati, Mounika S.K., “Application of Kalman Filter to Estimate Position of a Mobile Node in Indoor Environments,” May 2017, 55 pages. |
| Shala, Ubejd, et al., “Indoor Positioning using Sensor-fusion in Android Devices,” Sep. 2011, 58 pages. |
| Extended European Search Report for European Application No. 18907327.3 dated Sep. 29, 2021, 11 pages. |
| Number | Date | Country | |
|---|---|---|---|
| 20220378532 A1 | Dec 2022 | US |
| Number | Date | Country | |
|---|---|---|---|
| Parent | 15900552 | Feb 2018 | US |
| Child | 17110928 | US |
| Number | Date | Country | |
|---|---|---|---|
| Parent | 17110928 | Dec 2020 | US |
| Child | 17878670 | US |
