AUTOMATED WORKPIECE MACHINING BASED ON SENSED FORCES BETWEEN A TOOL AND A SURFACE OF A WORKPIECE

Abstract

Systems and methods relating to automated machining. A machining tool is controlled by an industrial robot that guides the tool around the contour of a workpiece. As the tool traverses the estimated contour of the workpiece, the tool machines the surface while a force sensor provides force readings, the force readings being readings of the force exerted by the tool on the surface. The path followed by the tool around the contour of the workpiece is adjusted based on the force readings. The speed at which the tool traverses the workpiece may be modulated and the path may be adjusted based on estimated tool wear.

Claims

1. A system for automatedly accomplishing machining tasks on a workpiece, the system comprising: a data processing subsystem for controlling an industrial robot coupled to a machining tool, said data processing subsystem being for controlling said machining tool to follow a contour of said workpiece; an imaging subsystem for imaging said workpiece, an image of said workpiece being used by said data processing subsystem to determine an initial path for said machining tool to follow around said contour; a force sensor coupled to a machining tool, said force sensor providing a force reading to said data processing subsystem, said force reading being an amount of force sensed by said force sensor; wherein said data processing subsystem adjusts a path for said machining tool based on said force reading; said machining tool machines said workpiece as said machine tool follows said contour.

2. The system according to claim 1, wherein said machine tool deburrs said contour of said workpiece as said machine tool follows said contour.

3. The system according to claim 1, wherein said imaging subsystem comprises at least one of: a laser scanner, one or more cameras, an x-ray based imaging device, or an acoustic based imaging device.

4. The system according to claim 1, wherein said data processing subsystem adjusts a speed at which said machining tool follows said contour.

5. The system according to claim 4, wherein said data processing subsystem modulates said speed when said force reading is outside a predetermined force range.

6. The system according to claim 1, wherein said data processing subsystem adjusts a speed at which said machining tool follows said perimeter contour based on differences between said path for said machining tool and said initial path.

7. The system according to claim 1, wherein said data processing subsystem adjusts a virtual stiffness parameter based on differences between said path for said machining tool and said initial path.

8. The system according to claim 1, wherein said data processing subsystem applies a velocity correction factor to thereby control a rate at which said path is adjusted.

9. The system according to claim 1, wherein said data processing subsystem adjusts said path based on a rate at which said machining tool wears.

10. A method for machining a workpiece using an industrial robot, the method comprising: a) obtaining an image of said workpiece; b) determining an initial path along a contour of said workpiece, said initial path being based on an estimated location of a surface of said contour of said workpiece, wherein a followed path initially conforms to said initial path, said followed path being a path to be followed by said robot with a machine tool along a contour of said workpiece to thereby machine said workpiece; c) receiving a force reading from a force sensor coupled to said machine tool, said force reading being an indication of a force exerted by said machine tool on said workpiece as said machine tool follows said surface of said contour of said workpiece; d) adjusting said followed path for said machine tool based on said force reading; e) repeating steps c) and d) as said machine tool traverses said contour.

11. The method according to claim 10, further comprising adjusting a speed at which said machine tool follows said followed path.

12. The method according to claim 10, wherein step d) is executed to maintain said force reading to be within a predetermined force range.

13. The method according to claim 10, wherein said machine tool traverses said contour at a constant speed.

14. The method according to claim 11, wherein said speed is modulated when said force reading is outside a predetermined force range.

15. The method according to claim 10, wherein said force is modeled as a spring and a spring constant is adjusted based on a difference between said initial path and said followed path such that when said difference is within a predetermined range, said followed path is less likely to deviate from said initial path.

16. The method according to claim 10, further comprising adjusting a virtual stiffness parameter based on differences between said followed path and said initial path.

17. The method according to claim 10, wherein said image is obtained using any one of: a laser scanner, one or more cameras, an x-ray based imaging device, or an acoustic based imaging device.

18. The method according to claim 10, wherein a contour of said workpiece is deburred as said machine tool traverses said contour of said workpiece.

19. The method according to claim 10, further comprising adjusting a speed at which said machine tool follows said followed path based on differences between said followed path and said initial path.

20. The method according to claim 10, further comprising applying a velocity correction factor to thereby control a rate at which said followed path is adjusted.

21. The method according to claim 10, further comprising adjusting said followed path based on a rate at which said machining tool wears.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

[0025] The embodiments of the present invention will now be described by reference to the following figures, in which identical reference numerals in different figures indicate identical elements and in which:

[0026] FIG. 1 is a diagram illustrating the coordinate systems used by the present invention as well as the various components used in an embodiment of the present invention;

[0027] FIG. 2 is a diagram detailing the nomenclature used for workpiece-tool interactions, with uncertain registration and tool size, as explained in accordance with one of the embodiments of the present invention;

[0028] FIG. 3 is a diagram also detailing the nomenclature for a setup according to another embodiment of the present invention;

[0029] FIGS. 4A-4D are illustrations of architectures of the various embodiments of the present invention;

[0030] FIGS. 5A-5B illustrate visualizations of workpiece-tool contact models as used in different embodiments of the present invention;

[0031] FIGS. 6A-6C illustrate the various variables and their physical counterparts for contour primitives;

[0032] FIG. 7 shows a feedrate/speed modulation curve in accordance with one embodiment of the present invention;

[0033] FIG. 8 is an image of an implementation of the present invention showing the various components of one aspect of the present invention; and

[0034] FIG. 9 is a flowchart detailing the steps in a method according to another aspect of the present invention.

DETAILED DESCRIPTION

[0035] To better understand the present invention, the reader is directed to the listing of citations at the end of this description. For ease of reference, these citations and references have been referred to by their listing number throughout this document. The contents of the citations in the list at the end of this description are hereby incorporated by reference herein in their entirety.

[0036] The present invention relates to systems and methods for machining a workpiece. In the detailed discussions below, a planar workpiece is machined to deburr the contour of the workpiece using an industrial robot controlled by a data processing device implementing a method according to one aspect of the present invention. In one implementation of the present invention, illustrated in FIG. 8, the system includes an industrial robot (a Denso VS-6556 manipulator), an imaging subsystem (a laser scanner and a camera system), a machining tool (the deburring tool), and a force sensor coupled to the robot and to the tool. As can be imagined, the operation of the manipulator/robot to machine the workpiece using the tool is controlled by the data processing subsystem. The computer vision subsystem (which may be the laser scanner or the camera subsystem by themselves) initially takes an image of the workpiece and this image is used to determine a path for the tool to follow around the contour of the workpiece. As the tool follows this initial path, the path followed by the tool is dynamically adjusted by the data processing subsystem based on the force sensed by the force sensor. Other machining tools other than a deburring tool may be used.

[0037] To better understand the explanation of the present invention, the reader is directed to the nomenclature explained below and in reference to FIG. 1 and FIG. 3.

[0038] The present invention relates to the robotic machining of the contours of planar parts. Let

[00001]$T_p^{*}$

denote the homogeneous transformation matrix between different frames, and let T.sub.p denote a known estimate of this transformation, obtained a priori via an imaging system, jigging, or similar. The machining tool is modeled as a cylinder of radius

[00002]$r_t^{*},$

and we let r.sub.t denote an a priori estimate of this quantity. The geometry of the part is assumed to be available in terms of primitives such as lines, arcs, and points, and it is assumed that a contouring path is planned around the part, as shown in FIG. 3, consisting of a time-series of 3D points p.sub.p, and unit-length normal direction vectors u.sub.n. Such path planning is known and references [1], [2], [3], and [4] may be consulted for convenience. Similarly, such path planning may be accomplished using various CAD/CAM software packages. An admittance controller is employed to correct for path errors by modifying the tool center along the normal axis u.sub.n by an admittance correction to maintain a desired normal force .sub.d from the measured force , such that

[00003]$\begin{array}{cc}{p}_t=p_p+a{u}_n,& \left(1\right)\end{array}$$\begin{array}{cc}\stackrel{.Math.}{a}=\frac{1}{m_a}\left[f_d-f-b_a\stackrel{.}{a}-k_{a}a\right],a=\stackrel{.Math.}{a}\mathrm{dtdt},& \left(2\right)\end{array}$

where m.sub., k.sub., and b.sub. are virtual mass-spring-damper coefficients used to tune the virtual suspension of the controller.

[0039] In one non-limiting embodiment, workpieces are assumed planar, and we define the part plane .sub.p from the xy plane of T.sub.p as

[00004]$\begin{array}{cc}{}_p=\left\{p{}^3|p=d_p+u{u}_{xp}+v{u}_{\mathrm{yp}},u,v\right\},& \left(3\right)\end{array}$

where u.sub.xp and u.sub.yp are the unit vectors along the x and y axes of T.sub.p, d.sub.p the translation vector of T.sub.p, and u, v are the planar coordinates of the point pnoting that, for this document, the underbar under a variable denotes 2D planar variables from their 3D counterparts. Projection of a point p.sup.3 to a point p.sub.p and back can be accomplished as

[00005]$\begin{array}{cc}\underset{}{p}={\left[\begin{array}{cc}{u}_{\mathrm{xp}}& u_{\mathrm{yp}}\end{array}\right]}^T\left(\underset{}{p}-d_p\right),& \left(4\right)\end{array}$$p=\left[\begin{array}{cc}{u}_{\mathrm{xp}}& u_{\mathrm{yp}}\end{array}\right]+d_p.$

[0040] Vectors are similarly converted without the shift of origin d.sub.p.

[0041] Within the plane .sub.p, the uncertainty of the workpiece and tool can be described using a four-state vector

[00006]$\begin{array}{cc}={\left[\begin{array}{cccc}x& y& & r\end{array}\right]}^T& \left(5\right)\end{array}$

where x, y, and are horizontal, vertical, and rotational workpiece corrections, and &r is a tool radius correction. This state vector defines the corrected part frame {tilde over (T)}.sub.p and tool radius r.sub.t=r.sub.t+ragain noting that in this document, the tilde above a variable denotes the current estimate of a variable, transformed by p, while those without represent the initial estimate.

[0042] In one aspect, this embodiment of the present invention modifies the admittance relationship in (2) by allowing for real-time adaptation of the workpiece registration and tool radius to minimize the corrections required by the controller. Simultaneously, a live covariance estimate is computed and leveraged to modulate the admittance tuning optimally. FIG. 4A illustrates the overall control architecture used in the present invention. A desired force .sub.d, target pose p.sub.p, and normal vector u.sub.n are provided by the path planner. A standard admittance controller modulates the target pose along the normal axis to maintain the target force. An inverse kinematics block converts the target pose to joint angles, which are sent to the joint servos. At the same time, an adaption block records the measured position p.sub.t and part interaction forces to update the state estimate and compute a path correction d.sub.cor, a rotational correction matrix R.sub.cor, and an admittance velocity correction term {dot over (a)}.sub.cor. Finally, an admittance tuning parameter .sub. is calculated from an estimate of the state covarianceallowing for the controller to behave rapidly before knowledge of the workpiece location is obtained, then transition naturally to be a stiff controller as the registration improves.

[0043] The architecture of the adaptation system 10 according to one aspect of the present invention is shown in FIG. 4B. First, the point accumulator 20 records the incoming planar tool coordinates p.sub.t and forces to form a list of all observations in the current run. The contact model system 30 estimates workpiece-tool contact points p.sub.ci from each observation. The CAD matching 40 relates the estimated contact points with corresponding points on the CAD model, p.sub.mi. The matched points and contact points are subtracted to form a set of residuals e.sub.i and used by the iterative refinement block 50 to compute state updates to p. The path correction system 60 computes d.sub.cor, R.sub.cor, and {dot over (a)}.sub.cor from the state estimate p. Finally, the covariance estimation system 70 estimates a state covariance matrix {tilde over (P)}, which is then used by the admittance tuning block 80 to compute an admittance tuning factor .sub..

[0044] Referring to the architecture of the aspect of the present invention that is illustrated in FIG. 4B, the architecture includes a point accumulator block. The point accumulator block's purpose is to build up a list of relevant observations, p.sub.ti and .sub.i, of workpiece-tool contacts. To avoid this list of observations from becoming overly large, two strategies may be used. First, an observation is only considered relevant (and thus logged) if the tool is in contact with the workpiecethis can be deduced by comparing the measured force .sub.i to a set threshold. Second, observations are logged at a lower frequency than the overall controller by downsampling the input, with appropriate anti-alias lowpass filtering, before processing.

[0045] Returning to the architecture in FIG. 4B, the architecture also includes a contact model block. The contact model block converts the set of n observed tool coordinates p.sub.ti and forces .sub.i into a set of n workpiece-tool contact points {tilde over (p)}.sub.ci. The estimation models the workpiece-tool contact as a spring of stiffness k.sub.t, as shown in FIG. 5. While k.sub.t is shown as the tool stiffness, in reality this stiffness combines the stiffnesses of the tool, the part and the manipulator. Accordingly, the block output {tilde over (p)}.sub.ci is

[00007]$\begin{array}{cc}{\underset{\_}{\tilde{p}}}_{\mathrm{ci}}={\underset{}{p}}_{\mathrm{ti}}+\left(r_t+r-f_i/k_t\right){\underset{}{R}}_{}{u}_{\mathrm{ni}},& \left(6\right)\end{array}$

[0046] with R.sub. as the 2D rotation matrix of the angle .

[0047] After the tool coordinate observations have been converted, the next step is to match each of the estimated workpiece-tool contact points {tilde over (p)}.sub.ci to their correspondences on the CAD model, denoted by p.sub.mi. First, the CAD model must be updated with the most recent state estimate .sub.k. The estimated correspondences can then be computed as the closest point on the CAD model. The CAD model is broken into basic primitives such as lines, arcs, and points, analogous to the DXF format commonly used in industry. For each entity, geometric formulae exist to compute these closest points analytically. For clarity, the methods used in [6] and [7] may be used.

[0048] We define the residuals of {tilde over (p)}.sub.i and p.sub.mi as e.sub.i={tilde over (p)}.sub.cip.sub.mi. Iterative refinement is accomplished by minimizing the nonlinear quadratic cost function

[00008]$\begin{array}{cc}L\left(\right)=\frac{1}{2}{}^T{P}_0^{-1}+\frac{1}{2}{.Math.}_{i=1}^n{\underset{}{e}}_i^T{S}_i^{-1}{\underset{}{e}}_i,& \left(7\right)\end{array}$

where P.sub.0.sup.44 and S.sub.i.sup.22 are positive definite matrices of the initial state covariance and the measurement covariance of the i th sample e.sub.i, respectively. The first quadratic term acts as an L.sub.2 regularization term, drawing the solution towards the initial state estimate in the absence of better data, and the second term minimizes the squared norm of the residual vectors. Each term is weighted by its respective reciprocal covariance

[00009]$\left(P_0^{-1}\mathrm{or}{S}_i^{-1}\right),$

ensuring that the resulting minimum is optimal, in the sense of minimizing the variance across all information sources.

[0049] Iterative refinement of L() can be accomplished using a second-order Newton step update at each controller cycle to reach the stationary optimal point where L()=0, as

[00010]$\begin{array}{cc}{}_k={}_{k-1}-{\left[{}^{2}L\left(\right)\right]}^{-1}L\left(\right),& \left(8\right)\end{array}$

where the associated gradient and Hessian are

[00011]$\begin{array}{cc}L\left(\right)=P_0^{-1}+{.Math.}_{i=1}^n{\underset{}{e}}_i^T{S}_i^{-1}{\underset{}{e}}_i,& \left(9\right)\end{array}$${}^{2}L\left(\right)=P_0^{-1}+{.Math.}_{i=1}^n\left[{\underset{}{e}}_i^T{S}_i^{-1}{\underset{}{e}}_i+{}^2{\underset{}{e}}_i{S}_i^{-1}{\underset{}{e}}_i\right].$

[0050] Equation (9) above requires that the residual Jacobian e.sub.i and Hessian .sup.2e.sub.i be computed. While these equations have been omitted for brevity, they are well-known in the field and to a person of skill in the art. Such equations can be derived analytically or estimated using a finite-differences approach.

[0051] As the state .sub.k is updated based on new observations from the real-time process, the original path can be corrected online with a translational adjustment d.sub.cor and a rotational adjustment R.sub.cor, visualized previously in FIG. 4A, as

[00012]$\begin{array}{cc}{p}_{p,\mathrm{cor}}=p_p+d_{\mathrm{cor}},& \left(10\right)\end{array}$$u_{n,\mathrm{cor}}=R_{\mathrm{cor}}{u}_n,$

where the 2D vector d.sub.cor describes a translation d=[x y].sup.T, a rotation by , and a translation of r along the admittance axis, as

[00013]$\begin{array}{cc}{\underset{}{d}}_{\mathrm{cor}}=\underset{}{d}+\left({\underset{}{R}}_{}{\underset{}{p}}_p-{\underset{}{p}}_p\right)-r{\underset{}{R}}_{}{\underset{}{p}}_p,& \left(11\right)\end{array}$

and the 3D vector d.sub.cor results by projecting d.sub.cor into 3D space as in Eqn. (4). The rotation matrix R.sub.cor is similarly defined as the 3D projection of the 2D rotation matrix R.sub..

[0052] In applying the path corrections, it is necessary to apply a velocity correction term {dot over (a)}.sub.cor to the admittance controller. Otherwise, the instantaneous path adjustments will be felt by the admittance controller and will result in undesirable forces applied to the part. By the chain rule, this velocity adjustment is

[00014]$\begin{array}{cc}{}_{\mathrm{cor}}=\frac{a}{t}={\left(\frac{a}{}\right)}^T\left(\frac{}{t}\right)=a^T\stackrel{}{},& \left(12\right)\end{array}$

where, for a sample time t.sub.s, the speed {dot over ()}=(.sub.k.sub.k1)/t.sub.s, and is computed by differentiating the projection of d.sub.cor onto the corrected admittance axis R.sub.u.sub.n as

[00015]$\begin{array}{cc}\begin{array}{c}a={\left({\underset{}{R}}_{}{\underset{}{u}}_n\right)}^T{\underset{}{d}}_{\mathrm{cor}})\\ ={\left[({\left({\underset{}{R}}_{}{\underset{}{u}}_n\right)}^T{\left({\underset{}{R^{}}}_{}{\underset{}{u}}_n\right)}^T\left(\underset{}{d}-p_p\right)-1\right]}^T,\end{array}& \left(13\right)\end{array}$

where

[00016]${\underset{}{R}}_{}^{}={\underset{}{R}}_{}/$

is the analytic derivative of R.sub..

[0053] It should be noted that, from this embodiment of the present invention, the covariance of the workpiece registration and tool, {tilde over (P)}, can be used to adaptively tune the admittance controller. This covariance matrix quantifies the uncertainty in the current state, given the observations, e.g., either the initial state or the residual samples. If we let z denote this set of generalized observations, then the state covariance can be computed by linearly projecting the observation covariance as

[00017]$\begin{array}{cc}\tilde{P}=\mathrm{cov}\left(\right)=\left(/z\right)\mathrm{cov}\left(z\right){\left(/z\right)}^T.& \left(14\right)\end{array}$

[0054] While the observation covariances P.sub.0 and S.sub.i are known, Eqn. (14) presents problems as the partial derivative term /z cannot be readily computed, and there exists only an implicit relationship between z and p defined by the optimization of L().

[0055] Using the Implicit Function Theorem and by judicious observations, Equation (14) can be simplified into the surprisingly elegant closed-form equation

[00018]$\begin{array}{cc}\tilde{P}={{\left[{}^{2}L\left(\right)\right]}^{-1}\left[P_0^{-1}+{.Math.}_i{e}_i^T{S}_i^{-1}{e}_i\right]\left[{}^{2}L\left(\right)\right]}^{-1},& \left(15\right)\end{array}$

where .sup.2L() and e.sub.i are as defined above, and all quantities are computed using the current state estimate, .sub.k.

[0056] It should be clear that, for the method used for the admittance, the optimal behavior is to respond quickly when the part location is unknown, but to be slow and become more stable as the certainty of the registration improves. The online state covariance estimate {tilde over (P)} gives an ideal tool for this modulation. By projecting the state covariance onto the current admittance axis as

[00019]$\begin{array}{cc}{}_a^2=\mathrm{var}\left(a\right)=a^T\tilde{P}a,& \left(16\right)\end{array}$

with as in Eqn. (13), we obtain

[00020]${}_a^2$

the variance of the admittance axis itself. This projected quantity is ideal since it allows the admittance to be modulated based on the local admittance conditions rather than the global convergence of the state parameters. In other words, the admittance can be tuned based on knowledge of whether the admittance correction is known, rather than on whether the whole part has been localized. This distinction allows for ideal admittance behavior. The admittance controller is adaptively tuned with a scaling parameter .sub., in the range 0-1, used in the modified controller

[00021]$\begin{array}{cc}\stackrel{.Math.}{a}=\frac{1}{m_a}\left[{}_a\left(f_d-f\right)-b_a\stackrel{.}{a}-k_{a}a\right],a=\left(\stackrel{.Math.}{a}\mathrm{dt}-{\stackrel{.}{a}}_{cor}\right)\mathrm{dt}.& \left(17\right)\end{array}$

[0057] The scaling parameter .sub. should be computed from

[00022]${}_a^2,$

according to the application. In one implementation, .sub. is computed according to the heuristic

[00023]$\begin{array}{cc}{}_a=1-\left(1-{\left({}_{a0}/{}_{2a}\right)}^{j_a}\right)/\left(1-{\left({}_{a0}/{}_{a,\min }\right)}^{j_a}\right),& \left(18\right)\end{array}$

where .sub.0 is the initial admittance standard error, .sub.,min is the minimum standard error (e.g., the error associated with no force control, and set according to the repeatability of the robot), and j.sub. is a tunable shape parameter. The result is then saturated within the range 0-1. While complex, this heuristic ensures that, as .sub.0 approaches .sub.,min, .sub. tends from one to zero, and the admittance becomes increasingly damped.

[0058] The results from the above embodiment of the present invention can, however, be improved upon. Experiments and further implementations show that, by adjusting the various parameters and concepts of the present invention, the following advantages over not using the present invention can be achieved: [0059] 1. Faster cycle times & reduced costs: Lower required workpiece registration accuracy allows the use of lower cost and faster localization methods. [0060] 2. Improved machining quality & faster feedrates: Adaptive controller tuning ensures an optimal trade-off between edge tracking performance and controller stability, improving surface finish and allowing faster feedrates to be used effectivelyfurther reducing cycle times. [0061] 3. Reduced tool wear: The present invention's improved contact stability reduces variability in the applied forces and notably reduces tool wearsignificantly lowering the ongoing cost of machining tools for the workcell. [0062] 4. Improved process awareness & tool wear tracking: The present invention provides real-time registration covariance estimates and tool radius estimates, increasing process visibility and allowing tool wear to be tracked run-to-run.

[0063] The embodiment of the present invention explained above relates to the robot machining of planar parts. An improved embodiment of the present invention can be characterized as relating to the optimal edge-tracking of a planar workpiece with uncertain registration. This improved embodiment takes into account tool wear and the variables and relationships are as shown in FIG. 2. As can be seen, the data shown in FIG. 2 is very similar to that in FIG. 3.

[0064] It should also be clear that, in the discussion below, some parts may be very similar to the discussion provided above. This is by design as the discussion below is an improvement on the embodiment of the present invention as detailed above.

[0065] As noted above, this embodiment of the present invention details optimal edge-tracking of a planar workpiece with uncertain registration. The associated frames and nomenclature are shown in FIG. 2. Let

[00024]$T_p^{*}$

denote the homogeneous transformation matrix between the world and part frames, and let T.sub.p denote a known estimate of this transform, obtained a priori via a vision system, jigging, or similar. The machining tool is modeled as a cylinder of radius

[00025]$r_t^{*},$

and we let r.sub.t denote an initial, potentially poor estimate of this quantity. While machining, the tool radius will change due to tool wear with a proportionality constant

[00026]${}_t^{*},$

and an initial estimate of this wear constant is .sub.t.

[0066] The geometry of the part is assumed to be available in terms of 2D primitive shapes, and it is assumed that a path allowing the tool to contour the part has been planned based on the estimated part locationas shown in dashed grey in FIG. 2. The workpiece geometry is constructed from a set of m primitive 2D entities, analogous to the DXF format commonly used in industry. In the current work, we consider three types of entity primitives: line elements, denoted as arc elements, denoted as and point elements, denoted as for each j-th entity. The point elements do not contribute to the part periphery; however, they are useful in the current analysis for dealing with corner features. Many shapes can be decomposed into these primitives, with point elements inserted at each entity boundary. For example, the part in FIG. 2 is decomposed into four line elements, one arc element, and five point elements at each intersection. The contouring path consists of a time-series of 3D tool center target points p.sub.p, unit-length normal direction vectors u.sub.n, and commanded forces .sub.d. As with the first embodiment of the present invention, a planning of such a path is well-known and may be found in the references noted above and may be accomplished using any number of CAD/CAM software packages.

[0067] As the robot contours the part, an admittance controller corrects for path errors by modifying the tool center from p.sub.p a distance along u.sub.n to the corrected tool center p.sub.t, as

[00027]$\begin{array}{cc}{p}_t=p_p+a{u}_n,& \left(1A\right)\end{array}$

and attempts to maintain the desired normal force .sub.d from the measured normal force according to the update law

[00028]$\begin{array}{cc}\stackrel{.Math.}{a}\left(t\right)=\frac{1}{m_a}\left[f_d\left(t\right)-f\left(t\right)-b_a\stackrel{.}{a}\left(t\right)-k_{a}a\left(t\right)\right],& \left(2A\right)\end{array}$$\stackrel{.}{a}\left(t\right)={}_0^t\stackrel{.Math.}{a}\left(\right)d,a\left(t\right)={}_0^t\stackrel{.}{a}\left(\right)d,$

where m.sub., k.sub., and b.sub. are virtual mass-spring-damper coefficients that tune the virtual suspension of the controller.

[0068] As can be seen, Eqns. (1A) and (2A) are very similar to Eqns. (1) and (2) above.

[0069] The initial transform T.sub.p comprises three unit vectors u.sub.xp, u.sub.yp, and u.sub.zp and a displacement vector d.sub.p as

[00029]$\begin{array}{cc}{T}_p=\left[\begin{array}{cc}{R}_p& d_p\\ 0& 1\end{array}\right]\left[\begin{array}{cccc}{u}_{xp}& u_{yp}& u_{zp}& d_p\\ 0& 0& 0& 1\end{array}\right].& \left(3A\right)\end{array}$

[0070] For this embodiment of the present invention, workpieces are assumed planar, and we define the part plane .sub.p from the xy plane of T.sub.p such that

[00030]$\begin{array}{cc}{}_p=\left\{p{}^3.Math.p=d_p+u{u}_{xp}+v{u}_{yp},u,v\right\}& \left(4A\right)\end{array}$

where p[u v].sup.T describes the planar coordinates of the point pnoting that, as above, the underbar denotes 2D planar variables from their 3D counterparts. Projection of a point p.sup.3 to a point p.sub.p, and vice-versa, is accomplished as

[00031]$\begin{array}{cc}\underset{}{p}={\left[\begin{array}{cc}{u}_{\mathrm{xp}}& u_{\mathrm{yp}}\end{array}\right]}^T\left(p-d_p\right),p=\left[\begin{array}{cc}{u}_{\mathrm{xp}}& u_{\mathrm{yp}}\end{array}\right]\underset{}{p}+d_p.& \left(5A\right)\end{array}$

[0071] Direction vectors are similarly converted, but without the translational shift of origin, d.sub.p.

[0072] Within the plane .sub.p, the uncertainty in the workpiece registration, tool radius, and tool wear constant can be described using the five-element state vector

[00032]$\begin{array}{cc}={\left[\begin{array}{ccccc}x& y& & r_t& {}_t\end{array}\right]}^T,& \left(6A\right)\end{array}$

where d[x y].sup.T is a translational and is a rotational workpiece profile correction, r.sub.t is a correction term for the tool radius, and .sub.t correction term for the tool wear constant. These states define the corrected part frame {tilde over (T)}.sub.p,

[00033]$\begin{array}{cc}{\tilde{T}}_p=T_p\left[\begin{array}{ccc}{\underset{\_}{R}}_{}& 0& \underset{\_}{d}\\ 0& 1& 0\\ 0& 0& 1\end{array}\right],{\underset{\_}{R}}_{}=\left[\begin{array}{cc}\cos & -\sin \\ \sin & \cos \end{array}\right],& \left(7A\right)\end{array}$

as well as the tool radius {tilde over (r)}.sub.t=r.sub.t+r.sub.t, and wear constant {tilde over ()}.sub.t=.sub.t+.sub.t. As with the explanation above, the tilde denotes the current variable estimate of a variable, transformed by , while those without the tilde represent the initial estimate.

[0073] This embodiment of the present invention uses a controller that modifies the admittance relationship by allowing for real-time adaptation of the workpiece registration and of the tool radius to minimize the work required by the admittance controller. Simultaneously, an estimate of the registration quality is computed and used to modulate the tuning of the admittance force controller as required by current process conditions. FIG. 4C shows a diagram of the control architecture for this embodiment of the present invention. The desired force .sub.d, target pose p.sub.p, and normal vector u.sub.n are provided by the path planner. A standard admittance controller modulates the target pose along the normal axis u.sub.n to maintain the target normal force .sub.d. Simultaneously, an adaptation block records the tool centers p.sub.t and forces f while in contact with the part, and uses this information to update the system state estimate , and to compute a path correction vector d.sub.cor, a rotational correction matrix R.sub.cor, and an admittance velocity correction term {dot over (a)}.sub.cor. Finally, an admittance tuning parameter .sub. is calculated from an estimate of the state covariance. This parameter allows for the controller to behave rapidly before knowledge of the workpiece location is obtained and then allows the controller to then transition naturally to a damped controller as the registration improves.

[0074] The system of this embodiment of the present invention can be divided into nine major subsystems, as illustrated in FIG. 4D. As can be seen, this system 10 is almost identical to that illustrated in FIG. 4B, with the exception that this embodiment includes a wear tracker block 80A and an outlier detection block 80B. The system in FIG. 4D has a point accumulator block 20 that collects the incoming tool center and force measurements, p.sub.t and , to form a list of all registered data during the current run. The wear tracker block 80A uses measured forces to estimate tool wear, and similarly collects this data in a list. The contact model 30 estimates the workpiece-tool contact points pp.sub.ci for each observation. The CAD matching system 40 takes the estimated tool contact points {tilde over (p)}p.sub.ci and matches them to their estimated corresponding points on the CAD model, p.sub.mi. These matched points can be compared with the contact points {tilde over (p)}.sub.ci to produce a set of residual vectors e.sub.i, which are used to compute weights w.sub.i in the outlier detection system 80B. The iterative refinement block 50 computes appropriate updates to the state estimate to minimize squared residual error. The path correction system 60 computes the path adjustment terms d.sub.cor, R.sub.cor, and {dot over (a)}.sub.cor. Finally, the covariance estimation system 70 computes the estimated state covariance P, and the admittance tuning block 80 projects this covariance onto the admittance axis to produce a tuning factor .sub. to modulate the admittance behavior based on the certainty of the path estimate. It should be clear that, where the subsystems of this embodiment are the same as the subsystems in the embodiment illustrated in FIG. 4B, details regarding the description, behavior, and implementation of such similar subsystems have been omitted. The subsystems that do not overlap for FIG. 4B and FIG. 4D are described in detail below as necessary.

[0075] The wear tracker block 80A seeks to model tool wear in real-time using the measured forces . While multiple models and approaches have been used to model tool wear, for this embodiment of the present invention, the wear model need only be accurate within the repeatability of the robotic manipulator, since toolpath accuracy will not improve past this level. Furthermore, part thickness, material and tool velocity are constant. Thus, a much simpler model can be used. The rate of change of the tool radius as proportional to the positive exponentiated applied force, is modeled as

[00034]$\begin{array}{cc}{\stackrel{.}{r}}_{t,\mathrm{wear}}={{\stackrel{~}{}}_t\left(f_i\stackrel{}{\underset{0}{}}\right)}^{j_t},& \left(8A\right)\end{array}$

where {tilde over ()}.sub.t is the estimated tool wear constant and j.sub.t is a shape parameter capturing the nonlinearity of the wear relationship. The symbol

[00035]$\stackrel{}{\underset{0}{}}$

denotes saturation between 0 and , since negative interaction forces do not cause negative tool wear. Thus, at a given time t.sub.i during the test, the tool radius correction due to wear is r.sub.t,wear={tilde over ()}.sub.t.sub.ti, where .sub.ti is an integrated force term, approximated with a sample rate F.sub.s as

[00036]$\begin{array}{cc}{}_{\mathrm{ti}}\stackrel{}{=}{}_0^{{}_i}{\left(f\left(t\right)\right)}^{j_t}\mathrm{dt}\frac{1}{F_s}\underset{k=1}{\overset{\text{?}}{.Math.}}{\left(f_k\right)}^{j_t}& \left(9A\right)\end{array}$$\text{?}\text{indicates text missing or illegible when filed}$

[0076] The forces .sub.k must be lowpass filtered before integration to remove frequency content higher than those representing actual mechanical part interactions. Appropriate estimates of the model parameters .sub.t and j.sub.t can be found via offline testing. To accommodate for manufacturing differences in individual tools and ensure optimal path accuracy, we include the wear constant {tilde over ()}.sub.t in the state vector , and allow it to be adapted in real-time based on observed data. However, it is deemed unnecessary to similarly allow for changes in the shape parameter j.sub.t. The applied force is generally constant, and thus j.sub.t is mostly unobservablewithout changes in , it is not possible to differentiate between a change in j.sub.t and a change in {tilde over ()}.sub.t. Thus, while j.sub.t is included to more accurately track wear, it is of little benefit to adapt it online.

[0077] If there are out-of-plane errors in part registration, the wear model may be inconsistent as the workpiece-tool contact shifts axially, causing wear at various tool points. However, often the part plane is constant, so accurate calibration is possible. Otherwise, dithering the tool along the spindle axis can reduce the required accuracy, while also extending tool life.

[0078] For the contact model block 30, this embodiment of the present invention takes into account the tool wear and, as such, the model used in the previous embodiment will need correction. For this contact model block 30, the block 30 converts each of the n measured tool positions p.sub.ti and forces .sub.i into corresponding estimated workpiece-tool contact points, p.sub.ci. The simple compliance model used for this estimation is shown in FIG. 5B, imagining a spring of constant k.sub.t between the workpiece and tool, while incorporating the radius correction r.sub.t and the wear correction {tilde over ()}.sub.t .sub.ti. Thus, an estimate of the contact point is

[00037]$\begin{array}{cc}{\widetilde{\underset{\_}{p}}}_{\mathrm{ci}}={\widetilde{\underset{\_}{p}}}_{\mathrm{ti}}+r_{t,\mathrm{cor}}{\widetilde{\underset{\_}{u}}}_{\mathrm{ni}}& \left(10A\right)\end{array}$

where the corrected tool radius is

[00038]$\begin{array}{cc}{r}_{t,\mathrm{cor}}=r_t+r_t-{\stackrel{~}{}}_t{}_{\mathrm{ti}}-f_i/k_t,& \left(11A\right)\end{array}$

and {tilde over (e)}.sub.ni is the estimated normal axis, after rotation by the current rotational state as .sub.ni={tilde over (R)}.sub.u.sub.ni.

[0079] For this embodiment, the tool stiffness k.sub.t is a lumped stiffness term that includes tool compliance, as well as the compliance of the robot itself. Thus, even rigid tools will have associated compliance. The stiffness term can be found via offline testing. Similar to the wear shape parameter j.sub.t, the stiffness k.sub.t is not included in the real-time state estimate . With a relatively constant force , it is not possible to differentiate between changes in the tool stiffness k.sub.t and changes in the tool radius r.sub.t, since each act collinearly in the estimation of {tilde over (p)}.sub.ci. Thus, small inaccuracies in the estimate of k.sub.t do not notably affect the method. Additionally, it was observed experimentally that k.sub.t does not change significantly with tool wear, and can be considered fixed for a given tool type.

[0080] The next step in the method for this embodiment of the present invention is to match each of the estimated contact points {tilde over (p)}.sub.ci to an estimate of their corresponding points on the CAD profile, denoted as p.sub.mi. First, the CAD entities must be updated according to the current state estimate .sub.k to give the estimated CAD entities {tilde over ()}. Then, correspondences are determined as the point on {tilde over ()} closest to the contact point {tilde over (p)}.sub.ci. This process is shown in FIG. 6A, FIG. 6B, and FIG. 6C and depends on the entity type: line, arc, or point.

[0081] If the j-th entity is a line element, then we label its original position and parameterize it as in FIG. 6A, with a starting point , a unit-length direction vector , and a line distance . This entity is transformed by the state estimate .sub.k to the estimated entity with parameters

[00039]$\begin{array}{cc}{\widetilde{\underset{\_}{c}}}_{}={\underset{\_}{R}}_{}{\underset{\_}{c}}_{}+\underset{\_}{d},{\underset{\_}{u}}_{}={\underset{\_}{R}}_{}{\underset{\_}{u}}_{}& \left(12A\right)\end{array}$

The closest point p.sub.mi is given by projecting {tilde over (p)}.sub.ci onto as

[00040]$\begin{array}{cc}{\begin{array}{cc}{\underset{\_}{p}}_{\mathrm{mi}}={\widetilde{\underset{\_}{c}}}_{}+{\widetilde{\underset{\_}{u}}}_{},& ={{\widetilde{\underset{\_}{u}}}_{}}^T\left({\widetilde{\underset{\_}{p}}}_c-{\widetilde{\underset{\_}{c}}}_{}\right)/d_{}\end{array}}^2.& \left(13A\right)\end{array}$

[0082] The intermediate vector is a number from 0 to 1, indicating the relative position of p.sub.mi along the line. If is outside of this range, the matched point is not on the line, and the closest point to {tilde over (p)}.sub.ci must lie on a different entity or the endpoints of the line, each represented by a separate point entity.

[0083] If the j-th entity is an arc element, we label its original position and parameterize it as in FIG. 6B, with an arc center c.sub., radius r.sub., and start and end angles .sub.1 and .sub.2, respectively. This entity is transformed by .sub.k to the estimated entity as

[00041]$\begin{array}{cc}\begin{array}{ccc}{\widetilde{\underset{\_}{c}}}_a={\widetilde{\underset{\_}{R}}}_{}{\underset{\_}{c}}_a+\underset{\_}{d},& {\stackrel{~}{}}_1={}_1+,& {\stackrel{~}{}}_2={}_2+.\end{array}& \left(14A\right)\end{array}$

The closest point {tilde over (p)}.sub.mi on to {tilde over (p)}.sub.ci is given by

[00042]$\begin{array}{cc}{\underset{\_}{p}}_{\mathrm{mi}}={\widetilde{\underset{\_}{c}}}_a+r_a{\widetilde{\underset{\_}{u}}}_m,& \left(15A\right)\end{array}$

where the unit direction vector .sub.m is obtained by normalizing the direction vector {tilde over (v)}.sub.m between {tilde over (p)}.sub.ci and the arc center {tilde over (c)}.sub.,

[00043]$\begin{array}{cc}\begin{array}{cc}{\widetilde{\underset{\_}{u}}}_m=\frac{{\widetilde{\underset{\_}{v}}}_m}{.Math.{\widetilde{\underset{\_}{v}}}_m.Math.},& {\widetilde{\underset{\_}{v}}}_m={\widetilde{\underset{\_}{p}}}_{\mathrm{ci}}-{\widetilde{\underset{\_}{c}}}_a.\end{array}& \left(16A\right)\end{array}$

[0084] This closest point will make an angle {tilde over ()}.sub.m=.sub.m with the center of the arc, which must be between the arc bounds {tilde over ()}.sub.1 and {tilde over ()}.sub.2. Otherwise, the closest point is on a different entity or the arc's endpoints.

[0085] Finally, if the j-th entity is a point element, we label its original position as . A point is parameterized as in FIG. 6C with the single 2-vector c.sub.p, and is transformed by .sub.k to the estimate entity with

[00044]$\begin{array}{cc}{\widetilde{\underset{\_}{c}}}_p={\underset{\_}{R}}_{}{\underset{\_}{c}}_p+\underset{\_}{d}.& \left(17A\right)\end{array}$

[0086] In all cases, the closest point p.sub.mi is simply equal to {tilde over (c)}.sub.p.

[0087] Once the closest point to each j-th entity, labeled as p.sub.mij, has been computed, it suffices to return the one that minimizes the Euclidean distance to {tilde over (p)}.sub.ci over each of the m entities,

[00045]$\begin{array}{cc}\begin{array}{cc}{\underset{\_}{p}}_{\mathrm{mi}}=\arg {\min }_{{\underset{\_}{p}}_{\mathrm{mij}},j=1,.Math.,m}& .Math.{\widetilde{\underset{\_}{p}}}_{\mathrm{ci}}-{\underset{\_}{p}}_{\mathrm{mij}}.Math..\end{array}& \left(18A\right)\end{array}$

[0088] The set of n estimated contact points {tilde over (p)}.sub.ci and their correspondences on the estimated workpiece profile, p.sub.mi, can now be readily used to refine the workpiece registration. Refinement is accomplished as an optimization, iteratively minimizing the nonlinear quadratic cost function

[00046]$\begin{array}{cc}L\left(\right)=\frac{1}{2}{}^T{P}_0^{-1}+\frac{1}{2}{.Math.}_{i=1}^n{w}_i{{\underset{\_}{e}}_i}^T{S}_i^{-1}{\underset{\_}{e}}_i,& \left(19A\right)\end{array}$

where e.sub.i is the error residual vector shown in FIG. 6A, FIG. 6B, and FIG. 6C,

[00047]$\begin{array}{cc}{\underset{\_}{e}}_i={\widetilde{\underset{\_}{p}}}_{\mathrm{ci}}-{\widetilde{\underset{\_}{p}}}_{\mathrm{mi}},& \left(20A\right)\end{array}$

and P.sub.0.sup.55 is the positive definite initial state covariance matrix, describing the accuracy of the poor initial registration estimate, S.sub.i.sup.22 is a positive definite matrix describing the covariance of the i-th measurement p.sub.ti, and w.sub.i is a weighting factor used for outlier detection.

[0089] This cost function contains two quadratic terms. The first term acts as an L.sub.2 regularization term, drawing the solution towards the initial state estimate in the absence of better data, and weighted by the reciprocal of the known initial covariance P.sub.0. The second term seeks to minimize the squared error of the residual vectorsagain weighted by the reciprocal of covariance S.sub.i. Weighting each term by their reciprocal covariance ensures that the resulting estimate is optimal, in the sense of minimizing the variance of all information sources. Iterative refinement of the cost function L() is accomplished with a second-order Newton step at each time step, .sub.k=.sub.k1+. We seek to satisfy the first-order optimality condition L()=0. A Taylor-series expansion of this condition from a perturbation is

[00048]$\begin{array}{cc}L\left(+\right)L\left(\right)+{}^{2}L\left(\right)=0.& \left(21A\right)\end{array}$

Rearrangement of Eqn. (21A) gives an estimate of the optimal step

[00049]$\begin{array}{cc}=-{\left[{}^{2}L\left(\right)\right]}^{-1}L\left(\right)=-{H\left(\right)}^{-1}g\left(\right),& \left(22A\right)\end{array}$

where g(.sub.k).sup.5 and H(.sub.k).sup.55 are the optimization gradient and Hessian, respectively, and are computed as

[00050]$\begin{array}{cc}\begin{array}{c}g\left(\right)=P_0^{-1}+{.Math.}_{i=1}^n{w}_i{\underset{\_}{e_i}}^T{S}_i^{-1}{\underset{\_}{e}}_i,\\ H\left(\right)=P_0^{-1}+{.Math.}_{i=1}^n{w}_i\left[{\underset{\_}{e_i}}^T{S}_i^{-1}{\underset{\_}{e}}_i+{}^2{\underset{\_}{e}}_i{S}_i^{-1}{\underset{\_}{e}}_i\right].\end{array}& \left(23A\right)\end{array}$

[0090] The first-order partial derivative terms of the residuals e.sub.i are 25 matrices, and the second-order partial derivatives .sup.2e.sub.i are 255 tensors, with the multiplication of .sup.2e.sub.i in Eqn. (23A) performed via a tensor contraction along the first dimensions of .sup.2e.sub.i and

[00051]$S_i^{-1}{\underset{\_}{e}}_{{}^i}.$

Equations to compute e.sub.i and .sup.2e.sub.i are required to implement this embodiment of the present invention. The derivation of these equations in full can be found in the Appendix of Reference [5].

[0091] The update scheme in Eqn. (22A) is highly efficient. As a pure Newton update, it converges quadratically once near a solution. Practically, this equation converges rapidly from poor initial state estimates to within robot tolerances in only two or three iterations. While efficient convergence is desirable, overly fast convergence could apply path corrections faster than the robot can respond. Simply saturating the state updates can destabilize the convergence. A more stable solution is to dampen the update equations as

[00052]$\begin{array}{cc}{}_{k+1}={}_k-{\left[H\left({}_k\right)+\mathrm{diag}\right]}^{-1}g\left({}_k\right),& \left(24A\right)\end{array}$

where is a 5-vector of positive damping factors for each state. This change has the effect of adding to the cost function L() an additional quadratic term (.sup.Tdiag), penalizing large rates of change in . Moreover, this addition has the advantage of helping to ensure that the inverted matrix in Eqn. (24A) remains positive definite, increasing algorithm stability.

[0092] Like all least-squares problems, the cost function L() can be sensitive to outliers. Because the squared error

[00053]${\underset{\_}{e}}_i^T{S}_i^{-1}{\underset{\_}{e}}_{{}^i}$

is very large for an outlier sample, the presence of only a few outliers can significantly skew the registration results. In the case of the present invention, outlier detection is critical since anomalous points regularly arise from two main sources. Firstly, burrs, scallops, or imperfections in the workpiece profile can cause the tool to deviate from the true part periphery and record erroneous points, which should be omitted from the analysis. Secondly, the contact model described above can give poor results near workpiece complexities such as corners or pockets. In these situations, the workpiece features may be smaller than the tool, and the side of the tool can contact the workpiece prior to the point along the normal vector u.sub.n. This situation is illustrated in FIG. 5B, where the right side of the tool contacts the workpiece earlier than expected due to the rotational error in the path, and thus the estimated contact point {tilde over (p)}.sub.ci is slightly off from the true contact point p.sub.ci. Without outlier detection, outliers would skew the results enough to notably reduce the algorithm's effectiveness.

[0093] However, this embodiment of the present invention uses an online identification routine, so outlier detection must be done carefully. As information is gathered, newer data points contain novel information, which may disagree with the current consensus and cause these points to be incorrectly categorized as outliers. When this happens, learning is delayed, and the method can malfunction.

[0094] To resolve this issue, we define two weighting schemesone to identify outliers based on probabilistical distance, with weights w.sub.di, and another to identify novel points and protect them from the outlier detection, with weights w.sub.ti. The weights, in the range 0-1, are combined using probabilistic OR logic, such that the point is included if either w.sub.di or w.sub.ti are large,

[00054]$\begin{array}{cc}{w}_i=1-\left(1-w_{di}\right)\left(1-w_{\mathrm{ti}}\right).& \left(25A\right)\end{array}$

[0095] The weights w.sub.di are determined by analyzing the residuals e.sub.i and excluding probabilistically unlikely points. This analysis is accomplished by assuming the residuals e.sub.i approximately follow a multivariate normal distribution of mean .sub.e and covariance .sub.e as

[00055]$\begin{array}{cc}{}_e=\frac{1}{n}{.Math.}_{i=1}^n{\underset{\_}{e}}_{{}^i},{.Math.}_e=\frac{1}{n}{.Math.}_{i=1}^n\left({\underset{\_}{e}}_{{}^i}-{}_e\right){\left({\underset{\_}{e}}_{{}^i}-{}_e\right)}^T.& \left(26A\right)\end{array}$

For each sample, the Mahalanobis distance is

[00056]$\begin{array}{cc}{d}_{mi}=\sqrt{{\left({\underset{\_}{e}}_{{}^i}-{}_e\right)}^T{{.Math.}_e^{-1}}_{\stackrel{}{}}\left({\underset{\_}{e}}_{{}^i}-{}_e\right)}& \left(27A\right)\end{array}$

and can be interpreted as a multi-dimensional generalization of how many (the number of) standard deviations per sample to the mean. A consensus is defined by thresholding points based on their Mahalanobis distance d.sub.mi using a logistic sigmoid function

[00057]$\begin{array}{cc}{w}_{di}={\left(1+e^{4\left(d_{mi}-b_d\right)/a_d}\right)}^{-1},& \left(28A\right)\end{array}$

where .sub.d and b.sub.d are tuning factors that determine the steepness and center of the thresholding, respectively, both in units of number of standard deviations, and the number 4 simply scales the slope of the sigmoid function such that 96% of the threshold transition happens within a distance .sub.d. Other scaling factors for the slope of the sigmoid functions may be used.

[0096] For time-based weights, the goal is to ensure that novel points are not identified as outliers. We define the age of the point as t.sub.i=t.sub.i, where t is the current process time, and t.sub.i is the time the point was gathered. The age is then thresholded as before

[00058]$\begin{array}{cc}{w}_{\mathrm{ti}}={\left(1+e^{4\left(t_i-b_t\right)/a_t}\right)}^{-1},& \left(29A\right)\end{array}$

where .sub.t and b.sub.t set the steepness and center of the threshold.

[0097] As noted above, the probabilistic OR logic is used to combine w.sub.di and w.sub.ti. The overall weight w.sub.i is high if either w.sub.di or w.sub.ti is highthat is, a point is included if it is not an outlier or if it is a recently obtained point.

[0098] As is updated during machining, the original path is corrected in real-time, as visualized in FIG. 4C, with

[00059]$\begin{array}{cc}{p}_{p,cor}=p_p+d_{\mathrm{cor}},u_{n,cor}=R_{cor}{u}_n.& \left(30A\right)\end{array}$

[0099] The 2D vector d.sub.cor comprises corrections due to a translation d, a rotation , and a radius-correcting translation along .sub.n,

[00060]$\begin{array}{cc}{\underset{\_}{d}}_{{}^{\mathrm{cor}}}=\underset{\_}{d}+\left({\underset{\_}{R}}_{{}^{}}{\underset{\_}{p}}_{{}^p}-{\underset{\_}{p}}_{{}^p}\right)+\left(r_t-r_{t,\mathrm{cor}}\right){\underset{\_}{R}}_{{}^{}}{\underset{\_}{u}}_{{}^n}.& \left(31A\right)\end{array}$

[0100] The 3D vector d.sub.cor can be computed from d.sub.cor with a coordinate conversion from planar space, as in Eqn. (5A). Lastly, R.sub.cor is defined by a rotation of about u.sub.zp, computed as

[00061]$\begin{array}{cc}{R}_{cor}=R_p\left[\begin{array}{cc}{\underset{\_}{R}}_{{}^{}}& 0\\ 0& 1\end{array}\right]R_p^T.& \left(32A\right)\end{array}$

[0101] In correcting the path, it is necessary to apply a velocity adjustment term {dot over (a)}.sub.cor to the admittance controller. Otherwise, the speed of the path adjustments from Eqn. (3A) will be felt by the controller, causing undesirable force fluctuations and destabilizing the force controller. The velocity adjustment term can be computed by leveraging the chain rule as

[00062]$\begin{array}{cc}{\stackrel{.}{a}}_{cor}=\frac{a}{t}={\left(\frac{a}{}\right)}^T\left(\frac{}{t}\right)=a^{{}_T}\stackrel{}{},& \left(33A\right)\end{array}$

where for a sample rate F.sub.s,

[00063]$\begin{array}{cc}\stackrel{}{}=F_s\left({}_k-{}_{k-1}\right)=F_s,& \left(34A\right)\end{array}$

and can be computed by differentiating d.sub.cor after projecting it onto the corrected admittance axis R.sub.u.sub.n as

[00064]$\begin{array}{cc}a=\left({\left({\underset{}{R}}_{}{\underset{}{u}}_n\right)}^T{\underset{}{d}}_{cor}\right)={\left[\begin{array}{cccc}{\left({\underset{}{R}}_{}{\underset{}{u}}_n\right)}^T& {\left({\underset{}{R}}_{}^{}{\underset{}{u}}_n\right)}^T\left(\underset{}{d}-{\underset{}{p}}_p\right)& \text{-1}& {}_{\mathrm{tk}}\end{array}\right]}^T,& \left(35A\right)\end{array}$

where

[00065]$R_{}^{}$

is the analytic derivative of R.sub. with respect to .

[0102] For the state covariance estimation, this embodiment of the present invention takes into account outlier detection. Accordingly, with the Hessian H as defined in Eqn. (23A), the closed-form estimate of state covariance is given as

[00066]$\begin{array}{cc}\tilde{P}=H^{-1}\left[P_0^{-1}+{.Math.}_{i=1}^n{w}_i^2{\underset{}{e}}_i^T{S}_i^{-1}{\underset{}{e}}_i\right]H^{-1},& \left(36A\right)\end{array}$

[0103] All quantities are computed with the current state, .sub.k. This can be contrasted with the closed-form estimate of state covariance for the previous embodiment as detailed in Eqn. (15) above.

[0104] For admittance controller adaptation, this embodiment of the present invention is very similar to that given and discussed for the previous embodiment detailed above. As with the previous embodiment, the optimal behavior of the admittance method is to respond quickly, with low damping, when the part location is unknown, but more stable as certainty of the workpiece edge improves. The online state covariance estimate {tilde over (P)} gives an ideal tool for this modulation. However, we are not interested in the variance of individual statesrather, we wish to know the certainty of the estimate of the admittance distance , as quantified by the variance

[00067]${}_a^2.$

This variance is computed by projecting the covariance {tilde over (P)} onto the current normal axis, as

[00068]$\begin{array}{cc}{}_a^2=\mathrm{var}\left(a\right)=a^T\tilde{P}a,& \left(37A\right)\end{array}$

where is computed as in Eqn. (35A). The variance

[00069]${}_a^2$

is a useful quantity since it allows the method of the present invention to adapt the admittance tuning according to the local rather than global uncertainty conditions. An example of this behavior is when the tool initially contacts the part along an edge. The algorithm will locate that edge, but it will not know if the errors are due to translational registration error or an incorrect tool radiuswithout data from multiple perspectives of the part, these two sources of error are collinear and unobservable. However, this collinearity is quantified by the state covariance estimate {tilde over (P)}, and when projected along this same axis, the admittance variance

[00070]${}_a^2$

will be low, which is the desired result.

[0105] Adaptation of the admittance controller is done via a scaling parameter .sub., in the range 0-1, which is used in the modified admittance force controller shown in FIG. 4D,

[00071]$\begin{array}{cc}\stackrel{.Math.}{a}\left(t\right)=\frac{1}{m_a}\left[f_d\left(t\right)-f\left(t\right)-\frac{b_a}{{}_a}\stackrel{.}{a}\left(t\right)-k_{a}a\left(t\right)\right],& \left(38A\right)\end{array}$$\begin{array}{cc}\stackrel{.}{a}\left(t\right)={}_0^t\stackrel{.Math.}{a}\left(\right)d,& a\left(t\right)={}_0^t\left(\stackrel{.}{a}\left(\right)-{\stackrel{.}{a}}_{cor}\left(\right)\right)d\end{array}.$

[0106] This controller is similar to the original admittance controller in Eqn. (2A), but the damping is modulated by the scaling parameter .sub.. Note that this adaptation scheme is updated slightly from scheme presented above in relation to the previous embodiment. However, this scheme has been adjusted to give better contact stability-for this embodiment, the mass ma and spring constant k.sub. were also modulated. However, experimental testing showed that better results could be achieved with constant mass and spring, and modulating only the damping based on registration accuracy. Because .sub. is in the range 0-1, the effective damping b.sub./.sub. of the controller can only increase. Thus, if the originally nominal admittance gains are safe, any value of .sub. will only slow down the system further, essentially tuning it asymptotically into a pure position controller as registration certainty improves.

[0107] The scaling parameter .sub. should be calculated from

[00072]${}_a^2$

based on the application to give reasonable values. In the current work, we propose the heuristic

[00073]$\begin{array}{cc}{}_a=1-\frac{1-{\left({}_{a0}/{}_a\right)}^{j_a}}{1-{\left({}_{a0}/{}_{a,\min }\right)}^{j_a}},& \left(39A\right)\end{array}$

where .sub.0 is the initial admittance standard error, which can be estimated by substituting P.sub.0 into Eqn. (37A); .sub.,min is the minimum standard error, i.e., the error associated with no force control, and be set to approximately the repeatability of the robot; and j.sub.a is a tunable shape parameter, with j.sub.a=0.5 being a reasonable starting estimate. Lastly, the value of .sub. is saturated between a lower bound .sub.,min and one to ensure safe behavior of the controller. While complex, this heuristic ensures that as .sub. approaches .sub.,min, the tuning .sub. tends from one to zero, and the admittance becomes increasingly damped, in a way that was found to give good behavior in physical testing.

[0108] The above embodiments of the present invention can, however, still be improved upon to provide even better results. More specifically, experiments have shown that, while the above embodiments of the present invention are effective, the machining of the workpiece can be further optimized such that, at least for a deburring task, burrs are removed more effectively. Adjustments to the present invention allow for, when the machining task is deburring, the identification and removal of large burrs, scallops, and index tabs on the workpiece. As well, these adjustments can more effectively remove large imperfections on the workpiece, improve controller contact stability, and optimize cycle times.

[0109] It should be clear from the above discussion that the present invention monitors the measured robot position q and tool forces and attempts to improve the registration estimate of the part while adaptively tuning the admittance controller for optimal performance. The system of the present invention outputs four key terms: [0110] 1. d.sub.cor, a path correction term based on an improved estimate of the workpiece location and tool size, [0111] 2. R.sub.cor, a rotational correction term for the orientation of the normal vector u.sub.n, based on an improved estimate of the workpiece rotation, [0112] 3. .sub., an adaptive tuning factor in the range 0-1, which represents the quality of the algorithm's estimate of the workpiece location, projected onto the current normal axis u.sub.n. When .fwdarw.1, the uncertainty of the path correction a is high, and when .fwdarw.0, the uncertainty is low, [0113] 4. {dot over (a)}.sub.cor, a velocity correction term applied in the admittance controller to compensate for changes in the path correction terms d.sub.cor and R.sub.cor.

[0114] The admittance controller defines the dynamic relationship between positional and force error to maintain the desired contact force .sub.d by modifying the commanded tool center a distance along the corrected normal axis .sub.n,cor, as shown in FIG. 5B, as

[00074]$\begin{array}{cc}{p}_{cmd}=p_p+a{\tilde{u}}_{n,cor}+d_{cor},{\tilde{u}}_{n,cor}=R_{\mathrm{cor}}{u}_n,& \left(1B\right)\end{array}$

[0115] where is computed with mass m.sub., spring constant k.sub., and damping dynamically modulated as b.sub./.sub., such that

[00075]$\begin{array}{cc}\begin{array}{cc}\stackrel{.Math.}{a}=\frac{1}{m_a}\left[f_d-f-\frac{b_a}{{}_a}\stackrel{.}{a}-k_{a}a\right],& a=\left({\stackrel{.}{a}}_{cor}+\stackrel{.Math.}{a}dt\right)dt.\end{array}& \left(2B\right)\end{array}$

[0116] When the part registration is poor, .sub.1, the effective damping is low, and the controller can move quickly to locate workpiece edges. However, as the localization improves, .sub..fwdarw.0, the damping increases, and the controller is slowed for better contact stability.

[0117] This third embodiment of the present invention adjusts the previous embodiments by [0118] 1. Modifying the admittance adaptation method to penalize positional deflections when part registration is good, thereby allowing for more complete removal of burrs, and [0119] 2. Introducing a feedrate modulation technique to more effectively remove large workpiece imperfections, improve controller contact stability, and optimize cycle times.

[0120] The following discussion below details these adjustments.

[0121] As should be clear, the present invention may be used for many machining tasks. When it comes to deburring, large burrs, scallops, index tabs, and other imperfections should be removed from the part edge. Unfortunately, complete removal of these imperfections can be challenging. Even larger burrs are small compared to the typical path accuracy of the manipulator, and without knowledge of the workpiece geometry, a nominal force-controlled deburring process cannot differentiate between a workpiece imperfection and the workpiece itself. However, the present invention offers two key advantages. First, the path error is reduced to negligible levels almost immediately once machining begins, enabling the detection and rejection of scallops and burrs. Second, the tuning parameter .sub. provides a reliable estimate of the current path accuracy. If the tool encounters an obstacle in the path and the path is estimated to be accurate, e.g., .sub..fwdarw.0, we can reasonably conclude that the obstacle is a workpiece imperfection that should be removed.

[0122] To improve the performance of the system of the present invention, a modification to the admittance update equation from Eqn. (2B) above can be applied. While the admittance adaptation term .sub. was previously used only to modulate virtual damping as b.sub./.sub., the virtual stiffness can be additionally modulated as

[00076]$\begin{array}{cc}\stackrel{.Math.}{a}=\frac{1}{m_a}\left[f_d-f-\frac{b_a}{{}_a}\stackrel{.}{a}-\frac{k_a}{{}_a^{\left[j_{k}1\left(-a\right)\right]}}a\right],& \left(3B\right)\end{array}$

where j.sub.k is a tunable shape parameter, and the Heaviside function 1() returns one when its input is positive and zero otherwise. Thus, the effective spring constant is k.sub. when the admittance correction a is positive, and k.sub./.sub..sup.jk otherwise. When the path estimate is poor, k.sub. must be tuned to a small, negligible value. Otherwise, the force controller would be discouraged from leaving the nominal, low-accuracy path, and result in poor edge-tracking. With the scheme defined in Eqn. (3B), this behavior is retained: if the path estimate is poor, .sub.1, and thus k.sub./.sub..sup.jkk.sub.. However, when the path estimate is good, we can penalize path deviations by setting k.sub. much higher. By dividing by k.sub./.sub..sup.jk as .fwdarw.>0, k.sub. becomes large. The parameter j.sub.k allows for more precise adaptation tuning based on the application, relative to the damping modulation b.sub./.sub..

[0123] Different behaviors may be desired depending on whether the admittance correction is positive or negative. If is negative, it indicates that the tool has moved outwards from the nominal path, and has likely encountered a burr, scallop, or other imperfection that should be removed. Thus, a large spring constant is ideal to prevent the tool from deflecting around the obstacle, and to fully remove it instead. However, if is positive, indicating that the tool has moved inwards from the nominal path, then the tool is following a pocket in the workpiece periphery. Although such a deviation of the part periphery is undesirable, the ideal outcome is to ensure that the edge inside this pocket is deburred appropriately. Thus, the desired behavior is to have a low spring constant, allowing the tool to deflect to follow the edge, despite the deviation. Including the Heaviside term 1() in the exponent of .sub. in Eqn. (3B) accomplishes this logic when a is positive by resetting the effective spring constant to be k.sub./.sub..sup.0=k.sub..

[0124] One potential downside of the above modification is that the contact force may increase substantially when encountering obstacles. However, this issue can be addressed by modulating the real-time feedrate (i.e. the speed at which the tool travels or traverses the contour of the workpiece). This is explained in more detail below.

[0125] The other modification to the present invention is to modulate the tool feedrate based on process conditions. This modification offers many advantages for precision robotic deburring applications. First, modulating the feedrate can improve cycle times. With constant feedrate, a conservative speed must be selected, dictated by the largest burrs or scallops on the part. By modulating the feedrate, more time can be spent on areas with greater need, and faster feedrates used otherwise. Second, feedrate modulation can enhance contact stability. There are situations where the tool loses contact with the part or applies excessive force. If the feedrate is held constant, sections of the workpiece may be unprocessed (in the case of contact loss) or damaged (in the case of over-force). Modulating the feedrate can slow the tool, giving the force controller more time to stabilize on the workpiece edge and avoid potential contact-loss or over-force errors. Finally, with the spring constant modulation adjustments detailed above, the tool will avoid deflecting around large workpiece imperfections such as burrs, scallops, or index tabs. Instead, the tool will push through the imperfection to remove it entirely. Unfortunately, this strategy, combined with a constant feedrate, causes the contact force to increase drastically, resulting in excessive tool wear. Moreover, the tool will only spend a short time on the burr, which may be insufficient to fully remove it. By adaptively slowing the feedrate, the force can be held more constant, and the tool will spend more time on the imperfection, removing it more effectively.

[0126] The feedrate modulation strategy depends on two main factors: the measured normal contact force error, .sub.err=.sub.d, and the accuracy of the path, as estimated by the admittance adaptation parameter, .sub.. The desired feedrate logic is as follows: [0127] 1. If the desired force is too low, then the tool has lost contact with the part. In this case, the feedrate should be slowed to allow the force controller to find the workpiece again without leaving unprocessed sections of the edge. [0128] 2. If the desired force is too high, then: [0129] (a) If the path accuracy is poor, i.e., .sub.1, then the tool is attempting to move into the part due to path error. In this case, the feedrate should be slowed to allow the force controller to move away from the part and stabilize the contact. [0130] (b) If the path accuracy is good, i.e., .sub.0, then the tool is in contact with an obstacle but will not move around it because of the logic from the above spring rate modulation. In these cases, the feedrate must also be slowed to lower the contact force while fully removing the workpiece imperfection.

[0131] Thus, despite various situations leading to changes in .sub.err, the solution always involves decreasing the feedrate when the force error magnitude increases, regardless of .sub..

[0132] For this embodiment, the feedrate modulation curve shown in FIG. 7 to implement the desired logic. Mathematically, this curve is described by the equation

[00077]$\begin{array}{cc}=\frac{{}_{nom}}{1+{.Math.\frac{f_{\mathrm{err}}}{b_{}}.Math.}^{1/a_{}}}& \left(4B\right)\end{array}$

where v.sub.nom is the nominal feedrate, and .sub.v and b.sub.v control the bell curve shape. The parameter .sub.v controls the steepness of the transition distance, with smaller values giving a steeper transition, and b.sub.v is the distance to

[00078]$v=\frac{1}{2}{v}_{nom},$

controlling the width of the bell curve.

[0133] The above modifications to the present invention that adjust the spring constant and the feedrate modulation enable the present invention to perform robotic deburring with greater contact stability, while simultaneously being more effective at removing peripheral imperfections such as large burrs, scallops, or index tabs.

[0134] Referring to FIG. 9, a flowchart detailing the steps in a method according to one aspect of the present invention is illustrated. As can be seen, the method 100 begins at step 110, that of obtaining an image of the workpiece to be machined. This step may be accomplished by imaging a planar view of the workpiece using a laser scanner, a computer vision subsystem, an x-ray based subsystem, an acoustic based imaging subsystem or any similar imaging subsystem. Once the workpiece has been imaged, an initial path around the contour of the workpiece is determined (step 120). This step can be accomplished using known tools and methods including well-known CAD/CAM software packages. After the initial path has been determined, then the tool (attached to the industrial robot/manipulator that will control the positioning and motion of the tool) begins to traverse the contour of the workpiece by following the initial path (step 130). While the tool is traversing the contour, a force sensor continuously takes force readings indicative of the force applied by the tool on the surface of the workpiece and these force readings are sent to a data processing subsystem (step 140). Based on the force readings, the path followed by the tool is adjusted so that a predetermined force reading is maintained (step 150). Step 140 and step 150 are repeated and looped until the contour of the workpiece has been machined.

[0135] As refinements to the general method, the speed of the tool may be constant or it may be modulated based on the change in position, the force readings, or any other relevant sensed parameters. Similarly, the followed path (i.e., the path followed by the tool around the contour of the workpiece) may be adjusted based on the wear rate/estimated wear of the tool.

[0136] As noted above, for a better understanding of the present invention, the following references may be consulted. Each of these references is hereby incorporated in their entirety by reference. [0137] [1] K. M. Murphy, R. J. Norcross, and F. M. Proctor, CAD directed robotic deburring, Proc. Second Int. Symp. Robot. Manuf. Res. Educ.Appl. Albuquerque, NM, 1988. [0138] [2] N. Asakawa, K. Toda, and Y. Takeuchi, Automation of chamfering by an industrial robot; for the case of hole on free-curved surface, Robot. Comput. Integr. Manuf., vol. 18, no. 5-6, pp. 379-385, 2002. [0139] [3] M. Jinno, M. Uenohara, J. Oaki, and K. Tatsuno, Teaching-less robot system for finishing workpieces of various shapes using force control and computer vision, IEEE Int. Conf. Intell. Robot. Syst., vol. 1, pp. 573-578, 1999. vol. 35, pp. 143-151, 2012. [0140] [4] K. Schtzer, E. Abele, and S. Gth, Simulation-based deburring tool and process development, CIRP Ann.Manuf. Technol., vol. 64, no. 1, pp. 357-360, 2015. [0141] [5] S. Lloyd, R. A. Irani, M. Ahmadi, Improved accuracy and contact stability in robotic contouring with simultaneous registration and machining, IEEE Trans. Control Syst. Technol. (2024) http://dx.doi.org/10.1109/TCST.2023.3348749. [0142] [6] Roth, G., & Levine, M. D. (1993). Extracting geometric primitives. CVGIP: image understanding, 58(1), 1-22. [0143] [7] Kaiser, A., Ybanez Zepeda, J. A., & Boubekeur, T. (2019 February). A survey of simple geometric primitives detection methods for captured 3D data. In Computer Graphics Forum (Vol. 38, No. 1, pp. 167-196).

[0144] It should be clear that the various aspects of the present invention may be implemented as software modules in an overall software system. As such, the present invention may thus take the form of computer executable instructions that, when executed, implements various software modules with predefined functions.

[0145] Additionally, it should be clear that, unless otherwise specified, any references herein to image or to images refer to a digital image or to digital images, comprising pixels or picture cells. Likewise, any references to an audio file or to audio files refer to digital audio files, unless otherwise specified. Video, video files, data objects, data files and all other such terms should be taken to mean digital files and/or data objects, unless otherwise specified.

[0146] The embodiments of the invention may be executed by a computer processor or similar device programmed in the manner of method steps, or may be executed by an electronic system which is provided with means for executing these steps. Similarly, an electronic memory means such as computer diskettes, CD-ROMs, Random Access Memory (RAM), Read Only Memory (ROM) or similar computer software storage media known in the art, may be programmed to execute such method steps. As well, electronic signals representing these method steps may also be transmitted via a communication network.

[0147] Embodiments of the invention may be implemented in any conventional computer programming language. For example, preferred embodiments may be implemented in a procedural programming language (e.g., C or Go) or an object-oriented language (e.g., C++, java, PHP, PYTHON or C#). Alternative embodiments of the invention may be implemented as pre-programmed hardware elements, other related components, or as a combination of hardware and software components.

[0148] Embodiments can be implemented as a computer program product for use with a computer system. Such implementations may include a series of computer instructions fixed either on a tangible medium, such as a computer readable medium (e.g., a diskette, CD-ROM, ROM, or fixed disk) or transmittable to a computer system, via a modem or other interface device, such as a communications adapter connected to a network over a medium. The medium may be either a tangible medium (e.g., optical or electrical communications lines) or a medium implemented with wireless techniques (e.g., microwave, infrared or other transmission techniques). The series of computer instructions embodies all or part of the functionality previously described herein. Those skilled in the art should appreciate that such computer instructions can be written in a number of programming languages for use with many computer architectures or operating systems. Furthermore, such instructions may be stored in any memory device, such as semiconductor, magnetic, optical or other memory devices, and may be transmitted using any communications technology, such as optical, infrared, microwave, or other transmission technologies. It is expected that such a computer program product may be distributed as a removable medium with accompanying printed or electronic documentation (e.g., shrink-wrapped software), preloaded with a computer system (e.g., on system ROM or fixed disk), or distributed from a server over a network (e.g., the Internet or World Wide Web). Of course, some embodiments of the invention may be implemented as a combination of both software (e.g., a computer program product) and hardware. Still other embodiments of the invention may be implemented as entirely hardware, or entirely software (e.g., a computer program product).

[0149] A person understanding this invention may now conceive of alternative structures and embodiments or variations of the above, all of which are intended to fall within the scope of the invention as defined in the claims that follow.