CANNON PIPER (US)
WEBSTER ROBERT (US)
HERRELL STANLEY (US)
WO2020228978A1 | 2020-11-19 |
US20210133496A1 | 2021-05-06 | |||
US11096753B1 | 2021-08-24 | |||
US7735390B2 | 2010-06-15 | |||
US20210031361A1 | 2021-02-04 | |||
US20210162591A1 | 2021-06-03 |
Claims We claim: 1. A robotic system comprising: a robot comprising a working portion configured to undergo robotic movement; a controller configured to command the operation of one or more actuators according to encoder values to cause the robotic movement in order to control the position the working portion; and an inertial measurement unit (IMU) configured to sense physical movements and to provide IMU data to the robot controller indicative of the sensed physical movements, the IMU being fixed to the working portion of the robot; wherein the controller is configured to fuse the encoder values with the IMU data to determine an estimated position of the robot. 2. The robotic system recited in claim 1, wherein: the robot comprises a robot arm with a plurality of links interconnected at joints so that the links can move relative to each other; the working portion comprises a portion of the robot arm; the operation of the actuators causes the links to articulate in order to control the position the robot arm; the IMU is fixed to the robot arm; and the estimated position of the robot comprises an estimated position of the robot arm. 3. The robotic system recited in claim 2, wherein the working portion comprises a tip of the robot arm. 4. The robotic system recited in claim 2, comprising multiple IMUs fixed to the robot arm at different locations. 5. The robotic system recited in claim 4, wherein an IMU is fixed to each link. 6. The robotic system recited in claim 2, wherein the joints comprise resolute joints. 7. The robotic system recited in claim 2, wherein the joints comprise prismatic joints. 8. The robotic system recited in claim 2, further comprising linkages connected to the links and the actuators, wherein the actuators are configured to manipulate the linkages in order to cause the links to move via the joints. 9. The robotic system recited in claim 8, wherein the linkages comprise at least one of cables, tendons, belts, gear trains, clutches, and linear actuators. 10. The robotic system recited in claim 2, wherein the robot comprises a serial robot. 11. The robotic system recited in claim 1, wherein: the robot comprises a movable member supported on a base by a plurality of actuators, the movable member being movable by the actuators relative to the base; the working portion is supported on and movable with the movable member; the operation of the actuators causes the movable member to articulate in order to control the position of the movable member; and the estimated position of the robot comprises an estimated position of the movable member. 12. The robotic system recited in claim 11, wherein the working portion is supported on the movable member. 13. The robotic system recited in claim 11, further comprising an IMU fixed to the movable member. 14. The robotic system recited in claim 11, wherein the actuators comprise prismatic joints. 15. The robotic system recited in claim 11, wherein the robot comprises a parallel robot. 16. The robotic system recited in claim 1, wherein the controller is configured to implement a recursive estimation algorithm configured to fuse the encoder values with the IMU data. 17. The robotic system recited in claim 16, wherein the recursive estimation algorithm is configured to statistically optimize the estimated position given the most recently obtained encoder values and IMU data and a previous best estimate of the estimated position. 18. The robotic system recited in claim 16, wherein the recursive estimation algorithm comprises a filter configured to fuse the encoder values with the IMU data. 19. The robotic system recited in claim 18, wherein the filter comprises a variable trust statistical filter that produces an estimated position of the working portion. 20. The robotic system recited in claim 18, wherein the filter is configured to bias the estimated position based on at least one of historical data and task- specific information comprising sensor noise, calibration, operating conditions, and past performance. 21. The robotic system recited in claim 16, wherein the controller is configured to execute the algorithm on an iterative basis in real-time to produce the estimated position in real-time. 22. The robotic system recited in claim 21, wherein the estimated position for a current iteration of the algorithm is biased based on an estimated position from a previous iteration of the algorithm. 23. The robotic system recited in claim 18, wherein the filter comprises a Kalman filter or a particle filter. 24. The robotic system recited in claim 1, wherein the working portion comprises an ultrasound probe secured to the robot arm, the IMU being fixed to the ultrasound probe. 25. The robotic system recited in claim 1, wherein the controller is configured to calibrate the encoder values with the IMU data by moving the working portion in open space free from interference from outside structures, wherein the IMU data is presumed accurate and is used to calibrate the encoder values in response to detecting a difference between the encoder values and the IMU data. |
C. Parameter Redundancy and Observability
[00103] There are currently 6(n+1 ) generalized error parameters in the robot kinematic model in Eq. 3. However, this is currently not a minimal set of parameters. In other words, there are directions in s-space which have no effect on the computed IMU location T in Eq. (3). Several of these redundancies are eliminated prior to calibration, using a known method, such as that disclosed in Meggiolaro and Dubowsky. If is the set of all generalized error parameters, the subset is the minimal set of parameters.
[00104] In Meggiolaro and Dubowsky, the authors provide a comprehensive and systematic approach for determining the subset depending on whether the end-effector pose measurements include both position and orientation. It is hypothesized (and numerical and real experiments agree) that when considering which parameters to eliminate from so, the 6 IMU measurements can be viewed as equivalent to the 6 pose parameters measured in a standard calibration. Therefore, based on Meggiolaro and Dubowsky, the length of the robot kinematic error vector is at most where N r is the number of revolute joints and N P is the number of prismatic joints.
[00105] Furthermore, it is important to note that in the IMU-robot arrangement, the IMU can only provide motion information relative to itself or relative to the robot. This means that the transform from the robot base to the world system is not observable from the IMU measurements. Because of this, the parameters are not included in Finally, the rotation parameters and which describe the orientation of the IMU frame are redundant with the each of the sensor orientations Therefore, these parameters are eliminated as well. After elimination of these redundant parameters, the length of [00106] Together with the robot parameters ε, 12 gyroscope parameters, 12 accelerometer parameters, 2 gravity direction parameters, and the time offset , there are system parameters that we pack into the vector . Table I details the components of the vector x. [00107] It has been previously shown that the transform from an IMU to a tracked object is observable (See, F. M. Mirzaei and S. I. Roumeliotis, “A kalman filter-based algorithm for imu-camera calibration: Observability analysis and performance evaluation,” IEEE Transactions on Robotics, vol.24, no.5, pp. 1143– 1156, 2008, the disclosure of which is hereby incorporated by reference in its entirety). In this disclosure, many more parameters are added to this problem. As opposed to proving observability in general, in Section VIII, observability of the system parameters ^^^^ is analyzed numerically for the specific robot setup. IV. BAYESIAN PARAMETER ESTIMATION
A. Problem Statement
[00108] In our method, instead of taking measurements at static configurations, the IMU is sampled while the robot moves continuously through the trajectory During robot motion, IMU outputs are sampled at times and the robot joint transducer outputs are sampled at times where and Nz, N q are the number of IMU measurements and the number of joint vector measurements respectively. Note that synchronous measurements of zt and qt are not assumed. The set of all IMU outputs zi.Nz and the set of all joint vector measurements are then used optimally to infer the system parameters x and the robot trajectory simultaneously. Here we derive an estimator, for example, roughly following P. Furgale, C. H. Tong, T. D. Barfoot, and G. Sibley, “Continuous time batch trajectory estimation using temporal basis functions,” The International Journal of Robotics Research, vol. 34, no. 14, pp. 1688-1710, 2015, the disclosure of which is hereby incorporated by reference in its entirety.
B. Posterior Parameter Distribution
[00109] All of the information that the measurements can reveal about the system parameters x and the robot trajectory is encoded in the posterior probability distribution Bayes’ Theorem asserts that this distribution takes the form
[00110] Assuming that the data and the system parameters x are all statistically independent from the inputs then the expression on the right becomes: [00111] Additionally assuming that the input measurements are independent of the IMU data z 1 N z leads to:
C. Conditional and Prior Distribution Assumptions
[00112] In order to make the estimation problem feasible, we must make statistical assumptions about the distributions of the measured data. As is often done, we assume a zero-mean, Gaussian measurement model for both the joint vector samples and the IMU outputs: where are zero-mean Gaussian random vectors with covariance and Here, the function and and is dependent on the function Equation (10) implies that the conditional distributions of the data are:
[00113] We also assume that the prior distribution of the system parameters are independent and Gaussian: where x are the nominal parameters and is the assumed covariance of the nominal parameters (e.g. from known measurement or manufacturing error).
[00114] The final assumption is that the prior distribution of the joint value function p(g)(t)) over the joint space is constant and uniform for all time. In other words, we assume that there is no prior information about the robot trajectory. Given these assumptions, the posterior distribution (Eq. 9) is proportional to a product of Gaussians where the constant of proportionality does not depend on either x or q(t) In particular, the proportionality can be expressed as:
D. Maximum A Posterion Formulation
[00115] The maximum a posteriori estimate seeks to minimize the negative logarithm of the posterior distribution, as this is equivalent to maximization of the distribution:
[00116] Given the Gaussian assumptions, Eq. 13 shows that the objective function in Eq. 14 can be expanded into the following quadratic form in the unknowns x and q(t): where c is some constant that does not depend on the system parameters x or the joint vector function q(t) and: e x — x x
[00117] Defining and because the constant c does not depend on the optimization variables, the solution to is the same as the MAP estimate (Eq. 14). Therefore, solving the optimization problem (Eq. 17) leads to the MAP estimate of the system parameters x and the trajectory
V. TRAJECTORY REPRESENTATION
[00118] Currently, along with the parameter vector x, one of the unknowns in the optimization problem of Eq. 17 is a function q(t). To compute a solution requires a representation of (t) that a computer can evaluate. Usually, continuous functions are approximated as a sum of basis functions, but the particular basis functions used are a design choice. For example, the basis functions disclosed in either of the following, both of which are hereby incorporated by reference in their entireties, can be used:
P. Furgale, C. H. Tong, T. D. Barfoot, and G. Sibley, “Continuous time batch trajectory estimation using temporal basis functions,” The International Journal of Robotics Research, vol. 34, no. 14, pp. 1688-1710, 2015.
P. Furgale, J. Rehder, and R. Siegwart, “Unified temporal and spatial calibration for multi-sensor systems,” in 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2013, pp. 1280-1286.
[00119] In doing so, B-splines are chosen as a basis to represent the unknown function q(t). There are two main advantages to this choice of basis for continuous-time batch estimation. One is that B-splines are locally supported. In other words, the contribution of any single basis function is local in time. This is useful for solving problems like Eq. 17 because the changes in the state trajectory caused by a change in a B-spline coefficient are zero almost everywhere. This makes for a sparse Jacobian and thus efficient nonlinear least squares solutions. Second, there are simple and efficient algorithms for evaluating B-spline functions, derivatives, and integrals - quantities that are necessary for evaluation of the objective function of Eq. 17. For example, the algorithms disclosed in such as L. L. Schumaker, Spline functions: computational methods. SIAM, 2015, the disclosure of which is hereby incorporated by reference in its entirety, can be used for evaluation of the objective function of Eq. 17.
[00120] The robot trajectory q(t) is represented as linear combinations of B- splines following the general methodology of Schumaker. Given spline degree d, smoothness d - 1, and time points in an interval the extended partition is defined to be: where the dimension of the spline space can be shown to be m = k + d + 1. Given this extended partition in one dimension, functions are represented as linear combinations of the normalized B-splines: where f is any function in the spline space, the are the normalized B- splines of order d + 1 and the are the multiplying coefficients which determine the shape of the function f(t). An example function is shown in Fig. 5.
[00121] Fig. 5 is a representation of a B-Spline function. The normalized B- splines are shown in color. The function which is a linear combination of the B-splines is shown in black. The B-splines make for efficient function evaluation and differentiation, and their local support makes for efficient solving of continuous-time trajectory estimation problems.
[00122] Extending this representation to n dimensions, we can write curves such as the robot trajectory in Eq. 17 using the following matrix equation:
[00123] As the basis functions are known, if the matrix C is known, then one way to calculate q(t) is by evaluating all of the B-splines that have value at the time point and then computing the linear combinations using Eq. 20.
[00124] However, a more efficient way to evaluate Eq. 20 is to use the recurrence relationship outlined in Schumaker. Additionally, we use a similar relationship (also outlined in Schumaker) to compute the B-spline coefficients corresponding to the derivatives of Eq. 20, from the matrix C.
[00125] Finally, as mentioned earlier, one important advantage of the B-spline representation is its local support. More specifically, when considering the interval
[00126] This is illustrated in Fig. 5, where each of the basis functions only take on nonzero values within a smaller subset of the total interval [0,1], This fact is exploited in Section VII where optimal trajectories are iteratively generated to maximize observability.
VI. LEAST SQUARES FORMULATION
[00127] Our ultimate goal is to solve Eq. 17, determining estimates for the parameters x as well as the unknown actual trajectory q(t). However, in light of the chosen trajectory representation of Eq. 20, the new unknowns to be estimated are x and C. Writing the coefficient matrix C as a vector: and defining the full vector of unknowns the cost function in Eq. 17 can be rewritten in matrix form as: where
[00128] The residual vector of Eq. 22 can be written as: where L is the Cholesky factor of Note that the Cholesky decomposition always exists in the case of symmetric, positive definite matrices like
[00129] If a unique solution exists, this problem can be solved with any nonlinear least squares solver such as the Levenberg-Marquardt (LM) algorithm. Furthermore if the solution is nearby to the nominal solution, the confidence in our estimate 0* is encoded in the posterior covariance matrix: where is the identification Jacobian matrix associated with 0. Note that least squares solvers like the LM algorithm generally benefit from providing a derivative to Eq. 23. In this case, assuming that L is constant, differentiation of Eq. 23 results in:
[00130] In our experiments, solution of Eq. 22 was not feasible with standard, dense matrices. Here, we analyze the sparsity structure of determining which elements of the Jacobian are nonzero. Knowledge of this structure enables the use of sparse finite difference methods to compute making solution of Eq. 22 feasible.
[00131] First, we note that of the parameters the system parameters x only influence the output measurement errors e z and cannot influence the input measurement errors e q . Inspection of Eq. 16 verifies this; e q is only a function of . In contrast, each component of the spline coefficients c can affect both and z . However, the local support of B-splines (Eq. 21 ) makes many of the elements o zero. For measurements q t taken at time
[00132] the derivative of Eq. 21 is zero. Employing the chain rule on in Eq. 16 shows that Similarly, the effect of is local on . A similar argument shows that for measurements taken at time Finally, a spline coefficient can only affect if it is on the same row in the matrix of Eq. 20, so can have only one nonzero element.
[00133] Next we note that the parameters in e which correspond to translational displacements cannot affect the orientation of the end effector. Therefore, the model predicted rate of orientation also cannot be affected by such length parameters, so that when As the gravity direction also does not affect measured orientations,
[00134] Because of the way the sensor misalignments incorporated into the matrices the corresponding matrices and have sparsity structures: where a * indicates any nonzero element. Similarly, it can be shown that the YZX Euler sequences r a and r w r! lead to the sparsity structure: for the matrices As the gain matrices are diagonal, and the sensor biases are just offsets, are all diagonal, 3x3 matrices. Finally, we note that the accelerometer parameters cannot affect the gyroscope outputs, and similarly, the gyroscope parameters cannot affect the accelerometer outputs; therefore, the following matrices are zero:
[00135] For is an identity matrix of size zero matrix with size ( p ) A diagram showing an example sparsity structure of the full Jacobian can be seen in Fig. 6. This diagram was generated using the system in experiments with 15 measurements of both q and z for visualization purposes. Note that with only 15 measurements in Fig. 6 is only about 85% sparse, but it was greater than 99% sparse in experiments where many thousands of measurements were taken.
[00136] Fig. 6. Sparsity pattern of the Jacobian matrix which can be exploited for efficient least squares solutions. The labeled, grey submatrices combine to form the full block matrix Black pixels indicate elements that are not necessarily zero, and grey pixels indicate entries that are always zero. In this example, only j data samples are used, for visualization purposes. In experiments, however, over 30000 samples were used, leading to a Jacobian with greater than 99% sparsity. VII. PLANNED TRAJECTORY OPTIMIZATION
[00137] Even though part of the problem is to estimate the actual robot trajectory q, up to this point, the commanded robot trajectory is still arbitrary. The actual robot trajectory q will in general differ from the plan q by some amount due to robot controller errors. While the joint values could be commanded to follow any trajectory (such as a sine wave), numerical experiments show that estimation error can be reduced substantially by commanding a trajectory q that is in some sense optimal.
[00138] Optimal trajectory determination has been used for identification of robot dynamic parameters for many years. (See, for example, J. Swevers, C. Ganseman, D. B. Tukel, J. De Schutter, and H. Van Brussel, “Optimal robot excitation and identification,” IEEE transactions on robotics and automation, vol. 13, no. 5, pp. 730-740, 1997, or V. Bonnet, P. Fraisse, A. Crosnier, M. Gautier, A. Gonz'alez, and G. Venture, “Optimal exciting dance for identifying inertial parameters of an anthropomorphic structure,” IEEE Transactions on Robotics, vol. 32, no. 4, pp. 823-836, 2016). According to the present disclosure, however, a new method is used, which specifically deals with long trajectories, such as the 5 minute timescale in the self-calibration problem. Instead of solving for the trajectory in a single step, the plan q is split up into many intervals. The information provided by all previous intervals is used to solve for the next segment of q within the current interval. This method is enabled by the local support of B-splines; as the effects of B-spline coefficients are local in time, changes in q within an interval can be achieved by altering only a few elements of c enabling sequential solutions.
A. Efficient Approximation of
[00139] In order to generate an optimal trajectory plan q, what is meant mean by “optimal” needs to be defined. The ultimate goal is to estimate the parameter vector x, so our definition for optimality should ultimately serve to reduce the posterior estimation error [00140] In order to develop an efficient approximation for it is momentarily assumed that the actual robot trajectory q is sufficiently close to the planned trajectory (i.e. that the actual trajectory is known, This assumption is reasonable because robot controllers are generally accurate at following planned trajectories. Furthermore, numerical results show that, even with this assumption, the parameter estimates are about 10 times more precise than if random trajectories are used. Note that the validity of this approximation is verified in the numerical experiments (See, Section VIII). The temporary assumption that q is not random enables efficient approximation of . When given a trajectory plan the posterior covariance of x takes the form where
[00141] Approximation of using (26) is significantly more efficient than computing with Eq. 24 and then extracting In the former case, we only need to compute a Jacobian of size 48 (assuming a 6-axis rotary system). In the latter case, we would have to compute the of size Because Nj and m are generally quite large (e.g. 36000 and 303 in the experiments), the matrix — is much larger in both dimensions than . Therefore, as long as the approximation of Eq. 26 is reasonably accurate, a significant efficiency advantage can be had by using Eq. 26 over Eq. 24 for the many thousands of computations of during trajectory optimization. The accuracy of Eq. 26 was validated with numerical experiments. The following paragraphs discuss using Eq. 26 to generate optimal trajectory plans q.
B. Trajectory Optimization Problem Statement
[00142] First, the posterior parameter uncertainty depends on the planned trajectory q to some extent as a stationary trajectory would not excite the system at all. To reduce posterior uncertainty, we want to make as small as possible in some sense by varying the B-spline coefficients c (and thus q(t)). This will make it so that after solving (22), the estimate of the parameter vector x will be as precise as possible given the potential constraints on q(t).
[00143] It is well-known that the maximum singular value of provides an upper bound on the posterior variance of any parameter in x. To reduce parameter uncertainty, we minimize the maximum singular value of In particular, to generate trajectories that maximize the observability of x, we solve the following optimization problem: subject to: where the vectors constrain the trajectory by setting upper and lower bounds on the joint values, velocities, and accelerations.
[00144] Solving Eq. 27 using the approximation of Eq. 26 should ensure a safe and (approximately) optimal trajectory during data collection. However, in order to achieve good results, the data collection process must proceed over several minutes. In experiments, this long trajectory corresponds to a c with length 1818. With an objective function (Eq. 27) taking several seconds to compute, a solution of this large scale, inequality constrained, nonlinear optimization problem is not feasible on a typical PC platform. Therefore, the method splits the problem up into many smaller optimization problems that can be solved sequentially.
C. Sequential Trajectory Optimization
[00145] To solve this large problem in many smaller sequential steps, we first partition the trajectory domain into many smaller intervals. The local support of B- splines makes it so that the trajectory in each interval is only affected by a small subset of the variable coefficients c. This fact is exploited to derive an efficient, sequential trajectory optimization scheme that could in principle be used for any system identification problem.
[00146] Toward partitioning the time domain, we first split the matrix C into several blocks is the number of columns in each of the blocks and is arbitrary. The matrix C is padded on the right and left with in which all of the columns are constant and equal; this serves to make the beginning and end of the trajectory have zero derivative values so that it is smooth. The rows of C are all initialized to be the mean values of the joint limits so that the nominal trajectory is centered at the middle of the joint space with no motion. During each iteration our algorithm will update the next columns of C given knowledge of all previously computed columns
[00147] Because of B-splines’ local support (Eq. 21 ), the columns Cj only have effect on the interval We could consider the information about x that sampling on this interval would yield. However, some of the affect of C overlaps into the interval affected by which is not desirable for solving for the trajectory sequentially. Therefore, we associate with the interval excluding the next interval Specifically, the interval that we associate with is:
[00148] Using the general results for recursively processing sets of measurements for estimation (see, e.g., P. S. Maybeck, Stochastic models, estimation, and control. Academic press, 1982, the disclosure of which is hereby incorporated by reference in its entirety), Eq. 26 can be decomposed into: where the covariance of x associated with interval is:
[00149] Here, e is the vector obtained by stacking all of the that happen to fall in the interval are all of the associated covariance matrices. The covariances in Eq. 31 can be interpreted as the uncertainty of the parameters x, given only the data sampled within the interval In Eq. 30, all of these intervals are brought together with the prior covariance to compute the posterior covariance
[00150] Next, we note that we can truncate the sum in Eq. 30 after only a portion of the N s intervals have occurred. This suggests the following recurrence relationship to compute the posterior covariance of x after j time intervals have occurred: where is the posterior covariance of x given measurements in all intervals up to and including 7 . The trajectory (and thus the data sampled) in is only dependent on the coefficients due to the locality of B-splines. Thus, the covariance is only a function of a small subset of the full parameter matrix C. Further, the construction of ensures that Cj will have no effect on the prior covariance which (given all has already been established in the previous iteration.
[00151] Therefore, instead of solving the optimization problem of Eq. 27 all at once, during each of the iterations, we instead solve the following local problem: subject to the same trajectory constraints (Eq. 28) as the global problem. Solving Eq. 33 for j = l...N s will give all of the columns of the full matrix C which should approximately solve Eq. 27.
[00152] During each iteration, this process can be thought of as adding onto the trajectory some new small trajectory piece over the added interval This additional trajectory piece only affects in the sum in (32). By changing , we design the information in this new interval to optimally combine with the prior, known information from the previous iterations. Note that the B-spline function representation ensures that the function (t) maintains continuity over its entire domain throughout this process.
[00153] This sequential approach may not lead to exactly the same solutions as directly solving Eq. 27. It is, however, significantly more efficient. Furthermore, in numerical experiments, trajectories generated in this way achieve parameter estimates that are 10 times more precise than using a random trajectory. Finally, because the approach essentially adds to the full trajectory a new small piece, it can be terminated once the covariance becomes sufficiently small which makes the method more flexible than solving Eq. 27 directly.
VIII. NUMERICAL EXPERIMENTS
[00154] A. Nominal System Parameters and Prior Uncertainties All of our numerical and real experiments are conducted with an AUBO i5 collaborative industrial arm (AUBO Robotics, USA) with an end effector-mounted Bosch BNO055 9-axis orientation sensor (see Fig. 12). Note that while this IMU is capable of estimating some of its parameters online, throughout our experiments, we are using it in the raw output mode collecting only raw triaxial accelerometer and gyroscope outputs. Note that while these outputs are generally voltages, our IMU instead outputs in inertial units converting using hardcoded gain values internally. While this does affect the units of our identified and nominal parameters (e.g. unitless sensor gains, biases with non-voltage units), it does not affect our method, as these parameters can be formed into an equivalent model. Here we justify all of the nominal parameters (and their prior uncertainties) related to our numerical and real experiments. A summary of this information is shown in Table II.
[00155] By definition, the nominal values for all of the robot kinematic errors e are zero, and as machining errors are generally on the order or 0.1 mm, we conservatively choose prior STDs of 1.0 mm for length parameters and 1.0! for angle parameters. Nominally, the gravity direction parameters g x and g y are zero as gravity should only be acting in the Z axis, but if the robot is mounted on the table with some angle 0 O , then the largest that either could be is 9.81 sin A conservative value for which implies a prior STD for and of about As we have no prior information about the time offset its nominal value is zero, and we conservatively choose a large prior STD of 1 second. The IMU board was oriented carefully onto the end effector at the nominal orientation for r a and r M . However, to account for potential mounting and machining errors, we choose prior STDs of 5° for the sensor orientations. The position of the IMU board relative to the end effector was measured with a set of calipers. To account for measurement error here, a prior STD of 10 mm was chosen for the parameters
[00156] The nominal values and prior uncertainties for the sensor parameters were chosen based on the IMU data sheet. For the triaxial accelerometer, the maximum cross axis sensitivity was quoted as 2%. This means that the maximum misalignment angle is sin -1 (0.02) = 1.146°, so we choose prior STDs of 1.5° for to be conservative. The maximum deviation of the accelerometer sensitivity was quoted as 4%. Therefore, we conservatively choose prior STDs of 0.1 for k a . The maximum zero-g offset was 150mg = 1.4715 m/s 2 , so conservative prior STDs of 2.0 m/s 2 were chosen for b a . The gyroscope cross axis sensitivity was quoted as 3%. Therefore, as sin -1 (0.03) = 1.7191°, we choose prior STDs of 2° for . The maximum deviation of the sensitivity was quoted to be 3%. Therefore, prior STDs of 0.1 for were chosen. The maximum zero-rate offset was quoted as 3°/s. Given this, prior STDs of 5°/s for were chosen. [00157] The standard assumption adopted throughout the experiments was that the parameters are not initially correlated. Therefore, the initial covariance of the parameters s diagonal; the elements on the diagonal of are then the squares of the initial standard deviations in Table 2.
[00158] Trajectory optimization (see, T. Qin, P. Li, and S. Shen, “Vins-mono: A robust and versatile monocular visual-inertial state estimator,” IEEE Transactions on Robotics, vol. 34, no. 4, pp. 1004-1020, 2018, , the disclosure of which is hereby incorporated by reference in its entirety) and calibration of the system (see, H. M. Le, T. N. Do, and S. J. Phee, “A survey on actuators-driven surgical robots,” Sensors and Actuators A: Physical, vol. 247, pp. 323-354, 2016, the disclosure of which is hereby incorporated by reference in its entirety) both require knowledge of the covariance matrices associated with the IMU measurements. Here, the standard assumption that the measurements are not correlated is applied again. Additionally, it is assumed that the covariance is constant during robot motion,
[00159] While can be estimated by sampling the IMU under static conditions, in our experiments, we found the measurement noise to be significantly greater when the robot is moving. Therefore, to determine Z z we moved the robot-IMU setup (see Fig. 12) dynamically while sampling the IMU. The samples z, collected over 300 seconds, were then least-squares fit to a spline function (see Eq. 20). The errors between the fit and the data were then used to compute the covariance 2 Z = diag[0.38 2 0.21 2 0.19 2 0.32 2 0.47 2 0.57 2 ] where the first three elements, corresponding to specific force variance, have units of (m/s 2 ) 2 and the last three elements have units of (°/s) 2 and represent angular velocity variance. Using similar methods and assumptions, the constant, diagonal covariance of the measured joint values is determined to be:
Zq = diag[0.0038 2 0.0050 2 0.0043 2 0.0105 2 0.0101 2 0.0086 2 ](°) 2 .
[00160] Fig. 12. Illustrates the experimental setup to test the calibration method’s ability to improve accuracy of the robot and sensor models. An aluminum plate with a Bosch BNO055 IMU and optical tracking spheres is mounted to the end effector of an AUBO i5 industrial collaborative robot. The NDI Polaris Vega optical tracker is used for ground truth data acquisition only to evaluate the calibration method’s ability to improve robot and sensor accuracy.
B. Planned Trajectory Optimization
[00161] In order to proceed with any of our other experiments, first we must choose the planned trajectory for the robot to follow during data collection. Here we discuss the details of implementing our trajectory optimization method (Section VII) with our particular robot-IMU setup. After optimization, we compare the performance of the optimal trajectory to a random trajectory of the same length to determine the performance gain by using the optimal trajectory.
[00162] We generated a 300 second trajectory using our recursive method outlined in Section VII. To build the matrices in Eq. 31 , we assumed a sample rate for the IMU and the joint values of 120 Hz as this was approximately the value achieved by our system. For the trajectory representation, we chose to use a spline of degree d = 3 with one interior knot per second. This leads to a C having 303 columns in total. Taking into account the known padding blocks C o and C Ns+1 , this leads to 297 columns of C to determine. We used a block size of ^^^^ ^^^^ = 5 columns per iteration. Given the 6 axis robot model, this leads to solving Eq.33 for ^^^^ ^^^^ = 60 iterations to determine the 1782 unknown elements of ^^^^. In order to constrain the motion (Eq.28) within safe operating conditions, the joint limits shown in Table 3 were used: [00163] During each iteration, a hybrid global optimization method was used to solve Eq.33 for the 30 elements of ^^^^ ^^^ ∗ ^. First, 2000 iterations of the simulated annealing algorithm were ran (implemented in MATLAB’s simulannealbnd function) to determine ^^^^ ^^^^ globally. This was followed by a local refinement step using the Hooke’s-Jeeves pattern search algorithm (as implemented in MATLAB’S patternsearch function) with 200 iterations, a maximum mesh size of 0.5, and compete polling. The optimal trajectory generated is shown in Fig.7, which illustrates the observability-optimal trajectory generated for the AUBO i5 industrial arm. The 5 minute trajectory is parameterized by 1818 B-spline coefficients. Solving for these optimal coefficients in the experiments directly was not computationally feasible. Use of the disclosed fast, sequential trajectory optimization method, however, made solutions possible. [00164] The trajectory performance measure (Eq.33) is shown decreasing with time in Fig.8, which shows the observability of the robot-IMU system parameters versus trajectory length. Compared with a random trajectory of the same length, the optimal trajectory generated by our sequential trajectory optimization method reduced the observability measure by at least an order of magnitude. This suggests that parameter estimates are 3-4 times more precise in the numerical experiments when using an optimal trajectory. Also shown here is the same observability measure computed using a random trajectory as opposed to the optimal trajectory. This random trajectory was determined by selecting uniformly random values for the matrix while still adhering to the trajectory constraints (see Eq.28). [00165] Additionally, using the relationship of Eq.32, the posterior standard deviations (the square roots of the diagonals of of all of the parameters in were computed. These are plotted versus time in Fig.9, which shows robot kinematic error parameters extrinsic system parameters d (middle); and IMU sensor parameters (bottom). The estimation precision of all parameters improves with trajectory length for this particular system. To verify the approximation of Eq.26, the approximate covariance was compared to the covariance predicted by the estimator of Eq. 24. After the full trajectory, the Frobenius norm between the two covariances was C. Monte Carlo Simulations [00166] To verify the observability of the parameters under our assumptions, we performed a series of Monte Carlo simulations. In each of the simulations, data was generated using ground truth parameters, noise was injected into the data, and then Eq.22 was solved using the noisy data to estimate the true parameters. In the following we describe this process for a single iteration. [00167] First, the ground truth set of parameters were sampled using the nominal values and standard deviations shown in Table II. Next, using the optimal trajectory the ground truth parameters, and the determined covariances the required data is generated using Eq.10. Using these measurements along with the nominal parameters as an initial guess, the estimate (Eq.22) and the posterior covariance (Eq.24) of the parameters This process was repeated for 100 simulations, and estimation errors for are shown in Fig.10 via histograms of errors between the 48 estimated and ground truth system parameters in the 100 Monte Carlo simulations. The red lines indicate the posterior distributions predicted by the estimator (i.e., Eq.24) which match closely with the histogram data. D. Robot Length Parameter Identifiability [00168] As discussed in Section X, the numerical results indicate that for the particular setup, robot length parameters (e.g., link lengths) cannot be estimated with much certainty. It is hypothesized that restrictions on robot joint speeds and accelerations (see Table III) could affect identifiability of robot length parameters. Here a numerical experiment is performed, analyzing the effect of trajectory speed on length parameter identifiability. [00169] The generated 300 second optimal trajectory was then used to define several faster trajectories. This was done by scaling the X−axis by different amounts so that the original trajectory was completed in less time. In this way, 5 new trajectories were created: one that was 2× faster than the original, one 4×, one 8× and so on. Using Eq.26, the posterior standard deviations of the robot length parameters were computed for each of the different trajectory speeds. We adjusted the 120 Hz sample rate to achieve the same number of data samples for each trajectory (e.g.240 Hz for the 2× trajectory). Posterior uncertainty of robot length parameters is shown decreasing with trajectory speed in Fig.11. [00170] In Fig.11, predicted posterior standard deviations of the 9 robot length parameters (e.g., link lengths) versus trajectory speed. As the speed of robot motion increases, the precision on length parameters improves substantially. While our robot’s motion was bounded by the values in Table 3, faster robots could enjoy accurate length parameter calibration with the disclosed method. IX. EXPERIMENTAL RESULTS [00171] To verify the ability of the disclosed method to increase robot accuracy and sensor accuracy, we performed a set of experiments testing our method with an AUBO i5 collaborative industrial arm with an end effector-mounted Bosch BNO055 9-axis orientation sensor (see Fig.12). Additionally, an NDI Polaris Vega optical tracker was used to measure the accuracy of the robot and the sensors. With the tracker, “standard” calibrations of both the robot and the IMU were performed separately for comparison with the estimated values. Note that the optical tracker was used for verification purposes only. [00172] The aluminum plate mounted to the end-effector flange served to hold the retro-reflective markers (for optical tracking) and the IMU board rigid with respect to the robot’s end effector. The IMU reported its samples to an Arduino Mega microcontroller using the I 2 C digital communication protocol. Software was written to command the robot along the desired optimal trajectory. Additionally, separate data collection software was written to simultaneously record the robot’s joint values (via MATLAB’s ROS toolbox) and the IMU sensor readings (via MATLAB’S serial interface). [00173] The robot was first commanded along the optimal trajectory while recording its joint values and the sensor readings on a separate PC. This information was then used to infer ^^^^ and thus the model parameters ^^^^ using Eq.22. The resulting values are shown alongside their nominal counterparts in Table 2. Based on the results of our numerical experiments in Section VIII, in real experiments, the choice was made to exclude robot length parameters from the calibration. This was achieved by setting their prior covariances to small numbers so that they would not be updated by calibration. Therefore, only the updated robot angle parameters are shown in Table 2. This limitation of our method is discussed thoroughly in Section X. [00174] Following the identification procedure, we began the robot accuracy evaluation process. The robot was next commanded to 250 discrete configurations for robot accuracy evaluation. Once at a configuration, the pose of the end effector was sampled for 3 seconds. The poses at a particular configuration were then averaged using the quaternion-based rotation averaging algorithm (see, F. L. Markley, Y. Cheng, J. L. Crassidis, and Y. Oshman, “Averaging quaternions,” Journal of Guidance, Control, and Dynamics, vol.30, no.4, pp.1193–1197, 2007, the disclosure of which is hereby incorporated by reference in its entirety). [00175] To verify our method’s ability to identify robot parameters, we performed a standard robot calibration (see Meggiolaro and Dubowsky, cited above) using 150 of the 250 collected poses. Note that, because the method could not calibrate length parameters accurately in the experimental setup, length parameters were also excluded in the standard calibration. The standard calibration values are shown alongside calibrated values in Table 2. The constant transforms necessary for evaluating robot accuracy were then extracted from the standard calibration. [00176] To test the method’s ability to improve robot accuracy, the remaining 100 poses were used to compute accuracy using nominal parameters, actual calibration parameters, and the standard calibration parameters. The positional accuracy was computed using the standard Euclidean norm between the tracker- measured and model predicted end effector positions. The rotational accuracy was taken as the standard minimal geodesic distance between the measured and predicted end effector rotations, . In other words, the rotation error is defined as the angle of the rotation These robot translation and rotation accuracy results are shown in Fig.13. [00177] Fig.13 shows histograms illustrating both position and rotation robot accuracy before calibration (top), after calibration according to the method disclosed herein (middle), and after a standard calibration (bottom). Accuracy is evaluated by comparing the model-predicted end effector pose to the optically- tracked ground truth. [00178] To verify our method’s ability to identify IMU parameters, we performed a standard IMU calibration using the optical tracker (see A. Kim and M. Golnaraghi, “Initial calibration of an inertial measurement unit using an optical position tracking system,” in PLANS 2004. Position Location and Navigation Symposium (IEEE Cat. No.04CH37556). IEEE, 2004, pp.96–101, the disclosure of which is hereby incorporated by reference in its entirety). The setup was manipulated along a random 20 minute trajectory while recording both tracker- based pose and raw IMU data. These data together were used for IMU calibration. Note that the angular parameters relative to the end effector were not included in the standard IMU calibration which was relative to the tracked markers. The standard calibration values are shown alongside our calibration values in Table 2. [00179] In addition to improving the robot accuracy, our method should improve the accuracy of the IMU sensors. Immediately after running the optimal calibration trajectory, a different random 120 second trajectory was run to evaluate the accuracy of the IMU sensors. During this process, the pose of the end effector was measured using the optical tracker for evaluation purposes. Given the alignment of the end-effector with the markers (known from the standard robot calibration above) and the alignment of the end-effector with the IMU (known from our calibration), the tracked pose of the IMU was computed. Splines of degree 5 and one interior knot per 30 samples were fit to the IMU position and quaternion data. These spline functions were differentiated to obtain ground truth IMU acceleration and angular velocity functions. The ground truth specific force was then computed from the acceleration data using the estimated gravity direction vector from the calibration In order to assess IMU calibration accuracy relative to the tracker data, the tracker must first be temporally aligned with the IMU. To accomplish this, first the tracker was temporally aligned with the robot data using a subset of the trajectory. Then the required tracker/IMU temporal alignment is just the sum of this tracker/robot alignment and the estimated robot/IMU alignment ^^^^ in Table 2. Using the temporal offsets, the ground truth specific forces and angular velocities were evaluated at the same time of the IMU samples and then transformed into the accelerometer and gyroscope coordinate systems respectively. [00180] The accelerometer and gyroscope IMU outputs were computed using the inverse of Eq.6 using nominal sensor parameters and our calibration parameters, and the standard calibration parameters. The normed differences between the sensor outputs and the ground truth inertial quantities measured with the tracker are shown in Fig.14 where a moving mean filter is applied with a window size of 500 samples. In Fig.14, histograms showing both specific force and angular velocity sensor accuracy before calibration (blue), after calibration according to the method disclosed herein (red), and after a standard calibration. Accuracy is evaluated by comparing the sensor model predicted values to the optically-tracked ground truth. Note that data has been passed through a moving mean filter with a 500 sample window. X. DISCUSSION [00181] Numerical experiments indicate that, for the particular IMU-robot arrangement disclosed herein, the system is observable. In optimal trajectory experiments (see Fig.9), all of the parameters’ STDs were reduced by calibration, and the identification Jacobian was found to be full rank and well- conditioned over the calibration trajectory. Furthermore, the Monte Carlo simulations indicate that, given all assumptions about the system and associated noise profiles, the disclosed calibration converges to the correct values. This indicates that the IMU and joint angle measurements provide enough information to infer ^^^^ for the disclosed setup at least in principle. The disclosed system employed a non-redundant manipulator; and it is noted that the results may not directly apply for a redundant setup and leave redundant robot/IMU calibration for future work. Furthermore, if observability is ever a problem for a specific setup, the disclosed Bayesian approach to parameter estimation ensures the existence of nonsingular solutions. In the case that the measurements cannot provide information about a particular set of parameters, their marginal prior and posterior distributions will be the same. [00182] This is the extreme case of the results for the robot length parameters. The numerical results suggest that the length parameters in ^^^^ are observable. This can be seen in Fig.9 where all of the predicted STDs of the robot length parameters were reduced given the measured data. However, while the angle parameter uncertainties were all reduced by at least a factor of ten, calibration only changed the length parameter uncertainties marginally. This suggests that, in this particular setup, calibration of the robot length parameters—while mathematically possible—is not practically feasible. For these reasons, it was decided not to include the robot length parameters as calibration variables in the real experiments. [00183] The identifiability of the robot length parameters could be improved with a longer trajectory, however, this comes with additional time and computation costs. As only the accelerometer measurements can give robot length parameter information, the lengths could be identified more accurately by improving the accelerometer signal to noise ratio. As mentioned previously, accelerometer noise was dominated by the vibrations of the system during trajectory following; therefore, we believe that using a higher fidelity IMU is unlikely to make a large difference. Instead, we propose that it is more practical to improve robot length parameter identifiability by utilizing faster robot trajectories. The real experiments were limited to the motion constraints in Table 3. The numerical experiments (see Fig.11), however, suggest that the identifiability of robot length parameters can be significantly improved with faster trajectories. Application of our method to high-speed robots for full Level II calibration (both length and angle parameters) could be the basis for future work (see Z. Roth, B. Mooring, and B. Ravani, “An overview of robot calibration,” IEEE Journal on Robotics and Automation, vol.3, no.5, pp.377–385, 1987, the disclosure of which is hereby incorporated by reference in its entirety). [00184] The problem proposed in this disclosure requires long robot trajectories in order to acquire enough information for a good calibration. Thus, trajectory optimization was used to ensure information richness of the robot motion, ultimately enabling shorter trajectories. Traditionally, trajectory optimization involves computing the entire optimal trajectory all at once. In numerical experiments, this approach led to an inequality-constrained, nonlinear optimization problem in 1782 variables. Due to this size and complexity, a solution was not feasible on a common PC platform. [00185] Alternatively, the sequential approach to trajectory optimization makes planning of such long, observability-optimal trajectories feasible by splitting the large-scale problem into many simpler ones. As shown in Fig.8, the trajectory optimization method significantly improved the observability of the system parameters ^^^^. The optimal trajectory yielded a posterior covariance of ^^^^ with a maximum singular value of 3.1e-05, whereas the random trajectory yielded a value of 4.6e-04. This implies that the posterior STDs are bounded by 0.0056 and 0.0215 respectively. Therefore, compared with a random trajectory, the optimal trajectory made the calibration 3.8 times more accurate for our system. It is noted that, based on Fig.8, adequate results are eventually possible with a random trajectory. In the experiments, however, an optimal trajectory is more efficient, achieving the same level of precision in less time. [00186] Monte Carlo simulations (see Fig.10) validates the method, given all assumptions about the system and the noise profiles of the sensors. Specifically, these results verify the approximation of Σ ^ in the trajectory optimization method. As shown in Fig.10, all of the predicted distributions based on the approximation of Eq.26 matched closely with the histograms output by the Monte Carlo simulations. Therefore, we can assume the approximation of Σ ^^ in Eq.26 to be reasonably accurate for our system. [00187] Real experiments show that the method works in practice to identify robot angle parameters. In Table 2, the average and maximum error between the identified angle parameters and the standard calibration was 0.04° and 0.09°, respectively. The method also significantly improved robot accuracy. This is shown in Fig.13 by the error reduction of both the position and rotation model predictions. Before calibration, the mean position accuracy was 1.58 mm. The calibration reduced this metric to 0.55 mm, near the 0.49 mm obtained by the standard calibration. Rotation accuracy was also improved by the method bringing the mean accuracy from 0.37° down to 0.13°, very near the 0.13° achieved by the standard calibration. [00188] All of this suggests that our method is comparable to a standard robot calibration for estimating angle parameters. Thus, our approach offers a cheap and fast Level I robot calibration (i.e., calibration of the joint angle offsets, see Roth, Mooring, and Ravani, cited above) as well as a partial Level II robot calibration (i.e., calibration of the entire rigid kinematics model). As previously noted, full Level II calibration could even be achieved given a sufficiently fast robot trajectory (see Fig.11), but this was not possible in the specific setup. The incapability of the method to estimate link lengths in the specific setup is likely not a large concern as an accuracy of 0.55 mm is adequate for most applications. Furthermore, it has previously been shown that for industrial arms, the majority of positional error is actually caused by errors in joint angle offsets rather than the other kinematic parameters (see P. Shiakolas, K. Conrad, and T. Yih, “On the accuracy, repeatability, and degree of influence of kinematics parameters for industrial robots,” International journal of modelling and simulation, vol.22, no.4, pp.245–254, 2002, the disclosure of which is hereby incorporated by reference in its entirety). [00189] In addition to robot angle parameter calibration the disclosed method also provides an IMU sensor calibration. All of the calibrated values match reasonably well with the standard IMU calibration in Table 2. The disclosed method also substantially improved the accuracy of the IMU sensors. Both angular velocity and specific force errors were reduced when compared to the optically tracked ground truths (see Fig.14). Specifically, over the 60 second evaluation trajectory, the angular velocity errors were reduced from 2.17 °/s to 0.95 °/s in the RMS sense. The standard calibration also achieved 0.95°. For the accelerometers, specific force errors were reduced from 0.93 m/s 2 to 0.77 m/s 2 in the RMS sense; the standard calibration also achieved 0.77 m/s 2 . [00190] All of this suggests that the disclosed method is comparable to a standard IMU calibration. Based on these results, and given that the disclosed method could be used with multiple IMUs at once, it is believed that the method could be useful in cases where many IMU sensors need to be calibrated quickly, such as in a sensor manufacturing facility. [00191] Finally, based on the Monte Carlo simulation, the disclosed method also accurately estimates all of the extrinsic parameters of the IMU (i.e., spatial offset, temporal offset, and gravity). This makes the disclosed method ideal as an initial calibration step enabling the use of IMU data in online robot estimation and monitoring applications. XI. CONCLUSION [00192] In this disclosure, a new method for jointly estimating robot kinematic parameters, inertial sensor parameters, and various extrinsic parameters relating the two (spatial offsets, temporal offsets, and gravity) is described. Testing and evaluation of the method is also described. Utilizing advancements in continuous-time batch trajectory estimation, the disclosure shows that the maximum a posteriori estimate leads to a nonlinear least squares problem and derives the sparsity structure of the associated Jacobian. Additionally, as long robot trajectories were required to achieve a good calibration, a new method is disclosed to generate observability-optimal trajectory plans sequentially, building the trajectory piece-by-piece. In the disclosed application, the results suggest that it achieves estimates that were many times more precise than a random trajectory. Note that generalization of the sequential trajectory optimization approach to many other estimation scenarios (e.g., robot dynamic identification See J. Swevers, C. Ganseman, D. B. Tukel, J. De Schutter, and H. Van Brussel, “Optimal robot excitation and identification,” IEEE transactions on robotics and automation, vol.13, no.5, pp.730–740, 1997 and K.-J. Park, “Fourier-based optimal excitation trajectories for the dynamic identification of robots,” Robotica, vol.24, no.5, p.625, 2006, the disclosures of which are hereby incorporated by reference in their entireties) is straightforward. Using this optimal trajectory, the unified calibration approach in a Monte Carlo simulation was evaluated, showing that the calibration produces the correct result on average. The numerical results also suggest a strong link between trajectory speed and robot length parameter identifiability. While length parameters could not be accurately estimated in the particular setup used in testing, application of the method to high-speed robots for full kinematic calibration could be the basis of future work. [00193] The disclosed method improved the accuracy of the robot in experiments substantially, suggesting a potential application in Level I robot calibration (i.e., determining joint angle offsets) as well as a partial Level II robot calibration (i.e., calibration of the entire rigid kinematics model). Furthermore, the method significantly reduced inertial sensor errors when compared to a ground truth showing promise for an alternative method of IMU sensor calibration. Additionally, experiments show that the method is comparable to standard methods for robot and IMU calibration. Based on the Monte Carlo simulation, the method also accurately estimated the extrinsic parameters of the IMU (i.e., the IMU translation, rotation, and temporal offset relative to the robot). This makes the method ideal as an initial calibration step enabling the use of IMU data in online robot estimation and monitoring applications. [00194] From the above description of the invention, those skilled in the art will perceive improvements, changes, and modifications. Notwithstanding such improvements, changes, and modifications, simply stated, the systems and methods disclosed herein can be applied to any robotic structure where knowledge of a precise location of the robot or any portion thereof is desired. According to the systems and methods disclosed herein, a recursive estimation algorithm can be implemented to fuse robot encoder values with IMU data to determine a statistically optimized estimated position of the robot, regardless of the robot’s configuration and/or construction. Such improvements, changes and modifications within the skill of the art are intended to be covered by the appended claims.
Next Patent: THREAT MITIGATION SYSTEM AND METHOD