Angular motion tracking sensor

[0001] This application claims priority from U.S. provisional application Ser. No. 60/812,470, filed on Jun. 9, 2006, incorporated herein by reference in its entirety.

[0002] Not Applicable

[0003] Not Applicable

[0004] A portion of the material in this patent document is subject to copyright protection under the copyright laws of the United States and of other countries. The owner of the copyright rights has no objection to the facsimile reproduction by anyone of the patent document or the patent disclosure, as it appears in the United States Patent and Trademark Office publicly available file or records, but otherwise reserves all copyright rights whatsoever. The copyright owner does not hereby waive any of its rights to have this patent document maintained in secrecy, including without limitation its rights pursuant to 37 C.F.R. §1.14.

[0005] 1. Field of the Invention

[0006] This invention pertains generally to the tracking of three-dimensional rotation, and more particularly to a real-time, non-contacting, three-dimensional, angular velocity sensor used in combination with a body rotating about a point which is fixed relative to the sensor.

[0007] 2. Description of Related Art

[0008] In the literature, there are a variety of devices based on a sphere rotating in a cradle. These include: spherical motors, spherical continuously variable transmissions, and omnidirectional vehicles based on the ball wheel mechanism. All of these devices require velocity feedback and would benefit from a non-contacting sensor that can track three-dimensional rotation. However, there is little work on spherical encoders or other means of three-dimensional, orientation feedback without mechanical coupling. In fact, there is no work that addresses three-dimensional, angular velocity feedback of a continuously rotating sphere without a prior knowledge of the rotation axis orientation.

[0009] Accordingly, the present invention is a three-dimensional angular motion sensor for a device or body rotating about a fixed point (spherical motion). The fixed point is not fixed in space—the point is fixed relative to the rotating body. Specifically, the absolute orientation of the body as well as the angular speed of the body, which includes the rate and the orientation of the axis of rotation, can be tracked without mechanical coupling to the body. The sensor itself requires measurement of the three-dimensional magnetic field of a dipole fixed on the body. With this data, the orientation of the dipole, the angular speed, and the orientation of the axis of rotation can be calculated.

[0010] The sensor is real-time, meaning that it only requires storing 3 sample points to make the calculation and that it involves no iterative algorithms or convergence methods. It is also critical that the sensor can track continuous rotation, like a tachometer.

[0011] An aspect of the invention is a motion tracking apparatus configured for real-time, three-dimensional angular motion tracking of an object; wherein the object is configured to engage in spherical motion.

[0012] Another aspect of the invention is a motion tracking apparatus configured for determining, in real time and in three dimensions, an angular velocity vector of a rigid body rotating about a fixed point.

[0013] Still another aspect of the invention is a motion tracking apparatus, comprising: a sensor; and means for determining an angular velocity vector for an object in real time; wherein the object is configured for spherical motion; and wherein the sensor is not in contact with the object.

[0014] One embodiment of this aspect further comprises a magnet coupled to the object; wherein the sensor comprises a tri-axial fluxgate magnetometer. In other embodiments of this aspect, the apparatus is configured for use in biomedical applications, in virtual reality and game applications, in military aircraft applications, or in robotic applications. In another embodiment of this aspect, the sensor is operable in harsh or demanding physical environments. In still another embodiment, the apparatus is configured for tracking continuous motion in real time. In yet another embodiment of this aspect, the sensor takes a single flux density measurement, in three dimensions, of the object; and components of the angular velocity vector are determined from the single flux density measurement.

[0015] Another aspect of the invention is a motion tracking apparatus configured for determining an instantaneous axis of rotation and an angular speed for a body from a single flux density measurement, in three dimensions, of the body; wherein the measurement is taken without physical contact of the body. In one embodiment of this aspect, the apparatus is configured to operate in complex ferromagnetic environments.

[0016] Another aspect of the invention is an apparatus, comprising: a rigid body configured for rotational movement in three dimensions; means for determining a trajectory of a point on the rigid body; means for determining an angular velocity vector of the rigid body from the trajectory; and means for outputting components of the angular velocity vector.

[0017] Another aspect of the invention is an apparatus, comprising: a rigid body configured for rotation in three dimensions; a magnet attached to the rigid body; means, not in contact with the rigid body, for measuring orientation of an axis of the magnet; means for computing real-time angular velocity data from the time-varying magnetic field measurements; and means for outputting the real-time angular velocity data for the rigid body.

[0018] Another aspect of the invention is an apparatus, comprising: means, not in contact with a rigid body, for motion tracking of the rigid body; means for measuring a three-dimensional trajectory of a point on the rigid body; means for determining an angular velocity of the rigid body from the trajectory; and means for outputting the angular velocity of the rigid body; wherein the rigid body is capable of rotational motion in three dimensions.

[0019] Another aspect of the invention is an apparatus, comprising: a rigid body configured for rotation in three dimensions; a magnet attached to the rigid body; means, not in contact with the rigid body, for measuring a time-varying magnetic field of the magnet; means for computing real-time angular velocity data from the time-varying magnetic field measurements; and means for outputting the real-time angular velocity data for the rigid body.

[0020] Another aspect of the invention is a method of tracking motion of a body, wherein the body is configured to engage in spherical motion, comprising: embedding a permanent magnet in the center of the body; measuring the time-varying magnetic field as the magnet rotates; determining the instantaneous magnet orientation from the field measurements; and outputting angular motion data for the body.

[0021] Another aspect of the invention is a method, comprising: measuring a three-dimensional trajectory of a point on a rigid body; determining an angular velocity of the rigid body from the trajectory; and outputting the angular velocity of the rigid body; wherein the rigid body is capable of rotational motion in three dimensions.

[0022] Further aspects of the invention will be brought out in the following portions of the specification, wherein the detailed description is for the purpose of fully disclosing preferred embodiments of the invention without placing limitations thereon.

[0044] Referring more specifically to the drawings, for illustrative purposes the present invention is characterized in FIG. 1 through FIG. 20. It will be appreciated that the apparatus may vary as to configuration and as to details of the parts, and that the method may vary as to the specific steps and sequence, without departing from the basic concepts as disclosed herein.

[0045] 1. Introduction

[0046] The present invention is a non-contacting, three-dimensional, angular velocity sensor based on magnetometry. The primary application for this sensor is the motion tracking of a robust omnidirectional wheeled mobile platform (OWMP), based on the ball wheel mechanism. This OWMP is developed for a compact, mobile manipulator that will be used for freeway maintenance and construction. The robust OWMP is designed to move at human walking speed, which corresponds to a maximum speed of approximately 1.8 m/s (4 mph). With the geometry of the prototype device, the corresponding ball wheel rotational speed is in the range of 0 Hz to 2 Hz. The admissible error is application specific. The sensor may be used in precision testing in a research environment or as a general purpose tachometer in a field vehicle. Each of these applications will require a different minimum accuracy. Because the OWMP is designed for operation in unstructured environments, the spherical tire will be subject to contamination and wear.

[0047] Information on the configuration of a magnetic source provides a means of determining the configuration of the body to which the sources are attached. In this case, the goal is to determine the axis of rotation and angular speed of the sphere given the trajectory of a vector fixed relative to the sphere, or equivalently, assuming a fixed sphere radius, the absolute position of a point on the sphere. This is essentially a problem of inverse kinematics. However, most approaches in the literature describe a process of calculating joint angles given the position of a reference point and the configuration of the rotation axis. For the ball wheel, both the configuration of the rotation axis and the angular displacement about this axis are unknown. Moreover, the values of these quantities can change arbitrarily and instantaneously. To surmount this problem properties of vector orthogonality are used to determine the axis of rotation or center of rotation of three-dimensional limb motion.

[0048] Development of the sensor consists of two main components: finding a means of non-contact motion tracking and solving the inverse kinematics problem of spherical motion about an unknown and arbitrary axis of rotation. Optical or vision techniques measure surface displacement, which can be used to derive the tangential surface velocity at a given location. In turn, this tangential velocity information can be used to determine the angular velocity and acceleration of a rigid body in two dimensions. For three-dimensional rotation, the inverse problem becomes ill-conditioned. More specifically, the matrix relating the three-dimensional angular velocity vector and the three-dimensional tangential velocity vector is skew-symmetric and its inverse is undefined. The present invention presents a method to directly calculate angular speed from position data.

[0049] Optical or image-processing techniques are common approaches for non-contact motion tracking. However, many optical devices, such as the optical mouse, require a relatively small sensor-to-surface gap, on the order of millimeters. It should be apparent that such a small gap leaves the sensor susceptible to damage or contamination from dirt and other debris. More sophisticated vision systems are based on edge or contrast detection which requires a surface grid or pattern. Both the surface-to-gap constraint and the surface pattern requirement are problematic in unstructured environments.

[0050] For non-contact motion tracking in demanding environments, magnetic sensing is commonly employed. Magnetic sensors have been used to track abdominal cavity deflection in crash test dummies as well as helmet motion tracking in aircraft cockpits, and for lateral vehicle guidance in industrial snow plows. These examples demonstrate that magnetic sensing can be effective in highly complex ferromagnetic environments.

[0051] As with most motion tracking schemes, the acquired data is a position or an orientation. Determining the angular velocity vector of the sphere is a separate matter. In the present application, the kinematic system can be described as rotation of a rigid-body about a fixed point. Consequently, all other points on the body are in spherical motion. The problem can then be recast as: determining the angular velocity vector of a rigid body given the trajectory of one of its points.

[0052] Literature on spatial kinematics or robotics treats this problem often, primarily for calculating joint angles from some desired end-effector position or orientation. Some closed-form and numerical techniques are presented in the literature. However, in all these techniques, the orientation of the rotation axes is known and each joint is limited to a single degree-of-freedom. For example, spherical wrists consists three revolute joints with each rotation axis orthogonal. As a result, each single degree-of-freedom joint can be tracked with an encoder.

[0053] Up until the last decade, there appeared to be no results relating a representation of a spherical motion and its angular velocity and acceleration. One source presents a treatment of three-dimensional, rigid-body rotation about an arbitrary axis of rotation, based on invariants of rigid-body transformations. These invariants are used to formulate rotation matrices for spatial kinematics problems. However, this formulation still requires a priori knowledge of the orientation of the rotation axis.

[0054] The solution to this inverse kinematics problem herein is based on the natural invariants noted above, but rather than using position data, the algorithm looks at the instantaneous heading of a point on the rigid body. As described below, there is a direct relationship between the axis of rotation and how the heading direction changes over a given sampling period.

[0055] It must be stressed that the present invention allows for the tracking of angular motion of a body including angular orientation and angular velocity. Accordingly, a wide variety of applications exist which include, but are not limited to, biomedical applications such as measuring limb motion, head orientation measurement for virtual reality and game applications, measurement of pilot motion such as heads and limbs in military aircraft applications, and a variety of robot applications.

[0056] 2. Magnetic Sensing

[0057] Development of the angular motion sensor consists of two main tasks: finding a means of non-contact motion tracking and solving the necessary kinematics problem to extract the desired data. As noted above, a magnetic sensing scheme is adopted for its robustness relative to optical methods. Generally, the approach involves: embedding a permanent magnet in the center of the sphere, measuring the time-varying magnetic field as the magnet rotates, and then determining the instantaneous magnet orientation from the field data. A diagram of the system is shown in FIG. 1 and variables used are summarized in Table 1.

[0058] The magnet is centered at origin O_M of the inertial reference frame and the sensor is centered at origin O_S. The magnet axis is defined by the unit vector {circumflex over (p)} and r is the position vector from O_M to O_S. The magnetometer measures the three-dimensional magnetic flux density vector of a cylindrically-symmetric ferromagnet, which will be modeled as an ideal magnetic dipole. Generally, the theoretical field equations are a function of six configuration variables and physical properties of the magnet. Furthermore, it will be assumed that the sphere and the magnet are both fixed in translation and both are perfectly centered at the origin of the inertial reference frame. For this analysis, {circumflex over (p)} will be represented with the direction cosines {l,m,n}.

[0059] In general, the flux density vector at O_S is three-dimensional. However, due to the cylindrical symmetry of an ideal dipole field, the flux density vector can be described using radial and tangential components in a plane containing {circumflex over (p)} and r. In FIG. 2, this plane is denoted by unit vector ê_n. Unit vectors ê_r and ê_t represent the radial and tangential directions, respectively. The relationship between the flux density vector and the configuration of the system is shown in FIG. 3.

[0060] The angle θ is the relative orientation between {circumflex over (p)} and r, in the ê_n-plane. The theoretical expressions for the radial and tangential field components, B_r and B_t, respectively, are

[0061] $\begin{array}{cc}{B}_r=\frac{{\mu }_{0}M}{2\pi {r}^3}\cos \theta & \left(1\right)\\ B_t=\frac{{\mu }_{0}M}{4\pi {r}^3}\sin \theta & \left(2\right)\end{array}$
where μ₀ is the permeability constant and M is the magnetic dipole moment. The relationship between the field components and the configuration variables can be found in most texts on electromagnetic fields.

[0062] In the three-dimensional case, the unit vectors {ê_r, ê_n, ê_t} are calculated as follows:

[0063] $\begin{array}{cc}{\hat{e}}_r=\frac{\underset{\_}{r}}{\underset{\_}{r}}& \left(3\right)\\ {\hat{e}}_n=\frac{{\hat{e}}_r⨯\hat{p}}{{\hat{e}}_r⨯\hat{p}}& \left(4\right)\\ {\hat{e}}_t={\hat{e}}_r⨯{\hat{e}}_n& \left(5\right)\end{array}$

[0064] Note that in Eq. 4, ê_r and {circumflex over (p)} are not orthogonal, in general; as a result, their vector product must be normalized to ensure that ê_n has unit magnitude. However, ê_r and ê_n are orthogonal at all times and the vector product to calculate ê_t does not require normalization. If the vector product of Eq. 4 is carried out, ê_t in Eq. 5 can be expressed as

[0065] $\begin{array}{cc}{\hat{e}}_t=\frac{1}{{\hat{e}}_r⨯\hat{p}}\left[\left({\hat{e}}_r·\hat{p}\right){\hat{e}}_r-\hat{p}\right]& \left(6\right)\end{array}$

[0066] Moreover, the trigonometric functions in Eq. 1 and Eq. 2 can also be expressed as a function of {circumflex over (p)}; as such,
cos θ=ê_r·{circumflex over (p)}  (7)
sin θ=∥ê_r×{circumflex over (p)}∥  (8)

[0067] For an arbitrary orientation of {circumflex over (p)}, the theoretical magnetic flux density vector, B_TH, can be expressed as
B_TH=B_rê_r+B_tê_t  (9)

[0068] Substituting Eq. 3, Eq. 6, Eq. 7, and Eq. 8 into Eq. 9 results in the following expression:

[0069] $\begin{array}{cc}{\underset{\_}{B}}_{\mathrm{TH}}=\frac{3{\mu }_{0}M}{4\pi {\underset{\_}{r}}^5}\left[\left(\underset{\_}{r}·\hat{p}\right)\underset{\_}{r}\right]-\frac{{\mu }_{0}M}{4\pi {\underset{\_}{r}}^3}\hat{p}& \left(10\right)\end{array}$

[0070] The geometry of the field equations is greatly simplified if the sensor is placed along one of the coordinate axes. Let the x-component of the magnetic sensor be aligned with the x-axis of the inertial reference frame; as such,
ê_r=[1,0,0]^T  (11)

[0071] Making this substitution, Eq. 10 becomes

[0072] $\begin{array}{cc}{\underset{\_}{B}}_{\mathrm{TH}}=\left[\begin{array}{c}\frac{{\mu }_{0}M}{2\pi r^3}l\\ \frac{-{\mu }_{0}M}{4\pi r^3}m\\ \frac{-{\mu }_{0}M}{4\pi r^3}n\end{array}\right]& \left(12\right)\end{array}$
which is an expression for the three-component flux density vector of a magnet defined by axis vector {circumflex over (p)}, represented by the direction cosines {l,m,n}. Assuming that theoretical flux density vector, Eq. 12, accurately represents the actual measured field, the direction cosines can be directly calculated from:

[0073] $\begin{array}{cc}\left[\begin{array}{c}l\\ m\\ n\end{array}\right]\approx \left[\begin{array}{c}\frac{2\pi r^3}{{\mu }_{0}M}{\left(B_x\right)}_M\\ \frac{-4\pi r^3}{{\mu }_{0}M}{\left(B_y\right)}_M\\ \frac{-4\pi r^3}{{\mu }_{0}M}{\left(B_z\right)}_M\end{array}\right]& \left(13\right)\end{array}$
where (B_x)_M, (B_y)_M, and (B_z)_M are the measured field components in the x, y, and z directions, respectively. The use of the approximation sign reflects general modeling error, such as modeling the permanent disc magnet as an ideal dipole. Eq. 13 states that the expression for the direction cosines of {circumflex over (p)} are uncoupled and linearly proportional to the field measurements. In other words, this scheme directly tracks the orientation of {circumflex over (p)}, and consequently the orientation of any rigid body to which the magnet is attached.

[0074] 2.1. Magnetometer Sensing Element Offsets

[0075] In the previous section, the x, y, and z sensing elements are assumed to be coincident. Physically, however, geometric offsets exist between the fluxgates, as shown in FIG. 4.

[0076] As a result, the position vector from the magnet origin to each fluxgate will differ. To address this issue, the theoretical flux density vector is calculated at the origin of each fluxgate and then projected along the active sensing direction. A diagram of planar offsets is shown in FIG. 5.

[0077] At this juncture, a vertical offset h of the sensor origin relative to the magnet origin is added. For tracking the motion of the ball wheel mechanism, the relative distance between the ball wheel and the chassis could change under deformation. However, it is assumed that this relative deformation is approximately constant during operation. In other applications, spatial constraints could prevent perfect alignment of the sensor origin and the magnet origin. As long as the offsets are known and constant, the variation in the signal can be accounted for.

[0078] The radial vector from the magnet origin to the origin of each sensor element is expressed as

[0079] $\begin{array}{cc}\left\{\begin{array}{ccc}{\underset{\_}{r}}_x=\left[\begin{array}{c}R\\ -d\\ -h\end{array}\right]& {\underset{\_}{r}}_y=\left[\begin{array}{c}R\\ 0\\ -h\end{array}\right]& {\underset{\_}{r}}_z=\left[\begin{array}{c}R\\ d\\ -h\end{array}\right]\end{array}\right\}& \left(14\right)\end{array}$

[0080] Next, the analysis in the previous section is carried for each fluxgate location. In summary, this process involves: attaching an orthogonal triad to each element origin, expressing the relative orientation between the magnet axis and the associated position vector, in terms of the direction cosines and geometric offsets, and substituting these values into the field equations; as such,

[0081] $\begin{array}{cc}{\left({\underset{\_}{B}}_{\mathrm{TH}}\right)}_X=\frac{3{\mu }_{0}M}{4\pi {{\underset{\_}{r}}_x}^5}\left[\left({\underset{\_}{r}}_x·\hat{p}\right){\underset{\_}{r}}_x\right]-\frac{{\mu }_{0}M}{4\pi {{\underset{\_}{r}}_x}^3}\hat{p}& \left(15\right)\\ {\left({\underset{\_}{B}}_{\mathrm{TH}}\right)}_Y=\frac{3{\mu }_{0}M}{4\pi {{\underset{\_}{r}}_y}^5}\left[\left({\underset{\_}{r}}_y·\hat{p}\right){\underset{\_}{r}}_y\right]-\frac{{\mu }_{0}M}{4\pi {{\underset{\_}{r}}_y}^3}\hat{p}& \left(16\right)\\ {\left({\underset{\_}{B}}_{\mathrm{TH}}\right)}_Z=\frac{3{\mu }_{0}M}{4\pi {{\underset{\_}{r}}_z}^5}\left[\left({\underset{\_}{r}}_z·\hat{p}\right){\underset{\_}{r}}_z\right]-\frac{{\mu }_{0}M}{4\pi {{\underset{\_}{r}}_z}^3}\hat{p}& \left(17\right)\end{array}$

[0082] Next, the theoretical flux density vectors are projected onto the active sensing axis of each fluxgate. These components are then expressed as
B_x=(B_TH)_X·î  (18)
B_y=(B_TH)_Y·ĵ  (19)
B_z=(B_TH)_Z·{circumflex over (k)}  (20)

[0083] Collecting the coefficients of the direction cosines into a matrix yields

[0084] $\begin{array}{cc}A=\left[\begin{array}{ccc}\frac{{\mu }_{0}M\left(3R^2-{{\underset{\_}{r}}_x}^2\right)}{4\pi {{\underset{\_}{r}}_x}^5}& -\frac{3{\mu }_0\mathrm{MdR}}{4\pi {{\underset{\_}{r}}_x}^5}& -\frac{3{\mu }_0\mathrm{MhR}}{4\pi {{\underset{\_}{r}}_x}^5}\\ 0& -\frac{{\mu }_{0}M}{4\pi {{\underset{\_}{r}}_y}^3}& 0\\ -\frac{3{\mu }_0\mathrm{MhR}}{4\pi {{\underset{\_}{r}}_z}^5}& -\frac{3{\mu }_0\mathrm{Mdh}}{4\pi {{\underset{\_}{r}}_z}^5}& \frac{{\mu }_{0}M\left(3h^2-{{\underset{\_}{r}}_z}^2\right)}{4\pi {{\underset{\_}{r}}_z}^5}\end{array}\right]& \left(21\right)\end{array}$

[0085] Then, the theoretical flux density vector, B_TH, can be expressed compactly as:
B_TH=A{circumflex over (p)}  (22)

[0086] A is a full rank, constant matrix; as such det(A)≢0. The unique solution is
{circumflex over (p)}=A⁻¹B_TH.  (23)

[0087] 3. Inverse Kinematics of Spherical Motion

[0088] As with many motion tracking schemes, the output of the proposed sensor is position. Since velocity is desired, the sensor data must be further processed; this includes numerical differentiation to find rates and kinematic analysis to resolve the three-dimensional velocity data to the orientation of the sphere's axis of rotation and the angular rate of rotation about this axis.

[0089] The present section describes the algorithm used to determine the angular velocity vector of a rigid body rotating about a fixed point. In general, the axis of rotation is three-dimensional and its orientation can change instantaneously and in an arbitrary direction. This is essentially an inverse kinematics problem. A diagram of the kinematic problem formulation is shown in FIG. 6. Consider a point p on the surface of sphere S, which is centered at origin O of an inertial reference frame. The position of p is defined by the vector r. As S rotates with angular velocity ω, point p follows a trajectory defined by the circular arc C with tangential velocity v_p.

[0090] Position vector r is represented by the direction cosines {l,m,n}, which are measured quantities from the described magnetometry scheme.

[0091] Next, the natural invariants of rigid-body rotation about a fixed point are derived. As shown in FIG. 7, the position vector r is decomposed into a component along the axis of rotation and a component normal to the axis rotation, r_∥ and r_⊥ respectively.

[0092] Explicitly, these components are
r_∥=(r·ê)ê  (24)
r_⊥=r−(r·ê)ê  (25)
where ê is a unit vector parallel to the angular velocity vector ω. Through this formulation, the natural invariants ê and θ are defined as shown in FIG. 8. The parameter θ represents the angular displacement of the normal component of the position vector as it rotates from r_⊥ to r′_⊥. This is corresponds to the motion of p to p′ along an arc length of curve C.

[0093] The relationship between the natural invariants and the motion of p can be defined as
r_⊥×r′_⊥=|r_⊥μr′_⊥|sin θê  (26)
where the angle θ is bounded by [0, π] to preserve the proper sign. However, according to Eq. 25, the projection r_⊥ cannot be made until ê is known. To circumvent this problem, some general orthogonality relationships of this motion are employed. For rigid-body motion, the tangential velocity vector v_p of point p is orthogonal to the position vector r at all times. As a result, v_p is also orthogonal to r_⊥. Consequently, the change in orientation of r_⊥ is equivalent to the change in orientation of v_p; as such,

[0094] $\begin{array}{cc}\frac{{\underset{\_}{r}}_{\perp }⨯{\underset{\_}{r}}_{\perp }^{\prime }}{{\underset{\_}{r}}_{\perp }{\underset{\_}{r}}_{\perp }^{\prime }}=\frac{{\underset{\_}{v}}_p⨯{\underset{\_}{v}}_p^{\prime }}{{\underset{\_}{v}}_p{\underset{\_}{v}}_p^{\prime }}& \left(27\right)\end{array}$

[0095] At this junction, a discrete analysis is taken, which better illustrates the numerical implementation of the proposed scheme. To do this, the sample index k and the sample period T are introduced into the equations. As a result, Eq. 26 becomes

[0096] $\begin{array}{cc}\frac{{\underset{\_}{v}}_p\left[k-1\right]⨯{\underset{\_}{v}}_p\left[k\right]}{{\underset{\_}{v}}_p\left[k-1\right]{\underset{\_}{v}}_p\left[k\right]}=\theta \left[k\right]\hat{e}& \left(28\right)\end{array}$
where the small angle approximation sin θ≈θ for θ small is made. Eq. 28 is the approximate radian rotation of Body S between each sample. If the samples occur over a period T, the approximate angular velocity vector can be expressed as

[0097] $\begin{array}{cc}\underset{\_}{\omega }\left(\left[k\right]T\right)\approx \frac{\theta \left(\left[k\right]T\right)}{T}\hat{e}& \left(29\right)\end{array}$

[0098] For the simplest implementation, a backward difference scheme is used to approximate the tangential velocity vector; as such,

[0099] $\begin{array}{cc}{\underset{\_}{v}}_p\left(\left[k\right]T\right)=\frac{{\underset{\_}{\delta }}_p\left(\left[k\right]T\right)}{T}\approx \frac{{\underset{\_}{r}}_p\left(\left[k\right]T\right)-{\underset{\_}{r}}_p\left(\left[k-1\right]T\right)}{T}& \left(30\right)\end{array}$

[0100] The term δ_p is the displacement of point p, described by position vector r_p over one sample period. Using this displacement variable, the angular velocity vector can be expressed as follows:

[0101] $\begin{array}{cc}\underset{\_}{\omega }\left(\left[k\right]T\right)\approx \frac{1}{T}\frac{{\underset{\_}{\delta }}_p\left(\left[k-1\right]T\right)⨯{\underset{\_}{\delta }}_p\left(\left[k\right]T\right)}{{\underset{\_}{\delta }}_p\left(\left[k-1\right]T\right)⨯{\underset{\_}{\delta }}_p\left(\left[k\right]T\right)}& \left(31\right)\end{array}$

[0102] Eq. 31 provides a method to approximate the angular velocity vector of Sphere S, given the absolute position of a point p and assuming the numerical differentiation scheme shown in Eq. 30.

[0103] While the differentiation operator is considered an ill-conditioned or ill-posed problem, its applicability is demonstrated extensively in the literature and in engineering practice. Applications include: edge detection of noisy images, tracking of noisy movie data, applications in radar motion tracking, applications in sonar motion tracking, and velocity estimation for control systems.

[0104] The approach taken here is based on numerical differentiation from the perspective of digital filtering. Generally, the goal is to build a filter that approximates the ideal differentiator at low frequencies, while attenuating the response at higher frequencies, which addresses the high frequency noise amplification. FIG. 9 shows the magnitude frequency response of several differentiators.

[0105] |H_ideal(ω)|=ω is the ideal differentiator, |H_fd(ω)| is the forward difference scheme, |H_cd(ω)| is the central difference scheme, and |H_diff(ω)| is a weighted, multipoint scheme that extends the linear range of the approximation. More specifically, H_cd(ω) effectively approximates the ideal differentiator up to a frequency of 0.08 f_s, while H_diff (ω) effectively approximates the ideal differentiator up to a frequency of 0.17 f_s, where f_s is the sampling frequency of the system.

[0106] 4. Experimental Verification Test Apparatus

[0107] The present section is concerned with experimental verification of the non-contact, angular motion sensing scheme. Generally, the approach involves spinning a magnet with a known angular speed and a known relative orientation and then using the magnetic tracking scheme to reproduce these known parameters. A diagram of the experimental set-up is shown in FIG. 10 and key experimental parameters are listed in Table 2.

[0108] The position vector r defines the position of the magnetometer origin O_S relative to the magnet origin O_M, which is centered in the inertial reference frame {î ĵ {circumflex over (k)}}. The field emanating from the magnet is defined by the orientation of its axis vector, denoted by vector {circumflex over (p)}. Coordinate q₁ is the relative orientation between the position vector r and the angular velocity vector ω. In summary, the angular speed ω and the angle q₁ will be regulated and simultaneously measured with conventional optical encoders and the proposed magnetometry scheme; this comparison will verify the effectiveness of the proposed scheme.

[0109] For this work, the axis of rotation will be constrained to the xy-plane; and as a result, the orientation of the rotation axis can be identified by the single angle q₁. Also, it is assumed that the magnet axis {circumflex over (p)} is orthogonal to the angular velocity vector ω at all times. This constrained system essentially replicates the kinematics of a steered wheel; as such, the proposed sensing scheme is a means of measuring the angular speed and steer angle of a steered wheel without mechanical coupling.

[0110] FIG. 11 is a diagram of the experimental test apparatus, illustrating the layout of the various components. The servo motor spins the magnet through an 8:1 gear reducer and is equipped with an incremental encoder for velocity feedback. The magnet spindle secures and precisely aligns the magnet relative to the rotation axis and the pivot center of the rotation arm. The fluxgate magnetometer is fastened to the rotation arm though a precision spacer that aligns the magnet origin with the sensor origin in the {circumflex over (k)} direction. The pivoting assembly also maintains a radial distance of 30.48 cm (12.00 in) between O_M and O_S. The reference measurement for orientation angle q₁ is provided by an optical encoder, which is mounted beneath the base plate.

[0111] For the experiments, a fluxgate magnetometer is selected for its high sensitivity and low signal-to-noise ratio, as compared to Hall Effect sensors. Specifically, the sensor is an Applied Physics Systems (APS) 535 Fluxgate Magnetometer. Its robust packaging makes the APS 535 ideal for applications in non-ideal environments. Specifications for the sensor are shown in Table 3. The dipole source is a Neodymium Iron Boron Grade 42 disc magnet with a maximum radial flux density of 13,200 Gauss. Physically, the magnet is 2.54 cm (1 in) in diameter and 1.91 cm (0.75 in) thick.

[0112] For this work, the actual magnitude of the magnetic field is not critical; it is the relative magnitude of each sensing axis that determines the rotational motion. In fact, the signals are normalized to a unit magnitude, characteristic of direction cosines. However, a relatively large magnetic field provides an improved signal-to-noise ratio.

[0113] The magnet spindle is actuated by a brushless servo motor (Parker Compumotor BE342HJ) through a precision gear head from Bayside Motion Products. The motor is driven by a Compumotor Gemini GV servo drive in velocity regulation mode. The controls algorithm and all the digital and analog I/O is programmed in LabVIEW™ Real-Time and run on a National Instruments PXI-8146RT, which has a 266 MHz Pentium processor. The sampling frequency is set to 500 Hz. The sampling rate is limited in this system because the signals from the two reference encoders must be also be sampled in real-time for experimental verification. With the relatively low target speeds, aliasing is not a concern; however, a higher sampling frequency could improve the filtering characteristics of the system.

[0114] The servo motor does produce a measurable field but is static and independent of the velocity setting. Since there is relative motion between the servo motor and the magnetometer in this system, it is necessary to account for the field variation with coordinate q₁. To do this, the magnet is removed from the collar and field measurements are acquired as q₁ is varied over a range of π/4 radians. A quadratic polynomial fit is sufficient in replicating the ambient field as a function of q₁. In the algorithm, the ambient field components are subtracted from the complete signal, leaving behind the time-varying field measurements.

[0115] Consider a rotation of the pivot stage from q₁=0 to q₁=π/4. At the end of this motion, direction cosines l and m should be of the same magnitude and in phase, while direction cosine n should be unaltered. FIG. 12A shows the results without compensation; the magnitudes are all erroneous and DC offsets are introduced. FIG. 12B shows the same test with the compensation. Note that the magnitudes of l and m are as expected, the amplitude of n is constant through the motion, and all signals oscillate about zero. It is important to note that in a real application, the magnetometer would be fixed relative to the motor assemblies and such compensation will not be necessary.

[0116] While the APS 535 Magnetometer has some internal signal processing, the sampled data passes through a second-order, low-pass digital Butterworth filter to filter out any noise from the DAQ hardware. After the numerical differentiation, the tangential velocity vectors are normalized and the current and previous values of the unit tangents are used to calculate the rotation vector per Eq. 28. After the vector product, the signals are filtered again to eliminate any noise amplification from the calculation.

[0117] The phase angle between the î-component and the ĵ-component of the rotation vector represents the angle q₁, which is the relative orientation between the axis of rotation and the sensor frame. The magnitude of the rotation vector, normalized by the period between the successive samples of the unit tangents, is the angular velocity of the magnet. The speed should be equivalent to the motor speed as sensed by the motor encoder.

[0118] 5. Test Case Results

[0119] Here, two test cases are presented. The first case involves a steady precession of the angular velocity vector from q₁=−π/2 radians to q₁=−π/4 radians. Secondly, the sensor performance is tested under a step change in velocity. For each case, four plots are presented: the direction cosines of the motion; the differentiated direction cosines, which represents the tangential speed for a unit sphere radius; the magnitudes of q₁ and ∥ω∥; and the error in q₁ and ∥ω∥ between the proposed method and the optical encoder measurement of the same quantity. These test cases are merely examples according to the present invention, and are not intended to limit the invention to the parameters described herein.

[0120] 5.1. Velocity Vector Precession

[0121] The first result involves precession of the rotation axis from q₁=−π/2 radians to q₁=−π/4 radians. As shown in the diagram of the experimental set-up (FIG. 11), the precession involves manually displacing the rotation arm as the magnet is rotating at a constant speed of 0.5 Hz. The orientation of the arm is measured by the optical encoder mounted beneath the base plate. Recall that while the sensor is the body being displaced, its instantaneous orientation still represents the inertial reference frame. Consequently, the negative rotation of the sensor corresponds to a positive rotation of the rotation axis, with the same magnitude.

[0122] The measured direction cosines for this motion are shown in FIG. 13. Note that, initially, the l and n components are 90° out of phase and are of the same magnitude, while the m component has zero magnitude, as expected. After approximately 9 seconds the precession of the axis begins, reaching the final orientation at approximately 13.5 seconds. In this time, the magnitude of direction cosine l decreases while the magnitude of direction cosine m increases. In the final orientation, l and m are in phase and are of the same magnitude, which is equivalent to the result of the oriented rotation described above. Also, through this transformation, direction cosine n remains unchanged.

[0123] The differentiated direction cosines, shown in FIG. 14, are consistent, demonstrating the amplitude scaling by the rotation speed.

[0124] The calculated angle q₁ and the angular speed ∥ω∥ are shown in the upper and lower plots of FIG. 15, respectively. The error in q₁ and ∥ω∥ are shown in the upper and lower plots of FIG. 16, respectively. Generally, the proposed sensor is able to track the precession. However, the sensor performance is compromised; noise spikes in the transient region are in excess of 5% error.

[0125] 5.2. Step Change in Velocity

[0126] The present section is concerned with the tracking accuracy of the proposed magnetic sensor under a step change in velocity. For this analysis, the velocity will be stepped from ∥ω∥=0.25 Hz to ∥ω∥=0.5 Hz with q₁=−π/2 radians. The measured direction cosines for this motion are shown in FIG. 17. The step change occurs after approximately 10 seconds; note the distinct change in frequency of the signals. As expected, there is no change in the direction cosine magnitudes.

[0127] The differentiated direction cosines, shown in FIG. 18, are consistent, demonstrating the amplitude scaling by the rotation speed.

[0128] The calculated angle q₁ and the angular speed ∥ω∥ are shown in the upper and lower plots of FIG. 19, respectively. Generally, this result shows good correspondence between the proposed magnetic sensor and the reference signal from the optical encoders. As mentioned above, the motor cannot respond with an ideal step; there is an apparent transient response that is a product of the motor, gearbox, and magnet spindle inertias as well as the PI controller used to regulate the motor speed. However, it is apparent from FIG. 19 that the proposed sensor has the bandwidth to track this transient response.

[0129] The error in q₁ and ∥ω∥ are shown in the upper and lower plots of FIG. 20, respectively. With the low-pass, Butterworth filtering of the raw data, as well as the low-pass filtering effects of the numerical differentiation filter, there is lag and magnitude attenuation of the output from the magnetic sensor scheme. As a result, there is a noticeable error across the step. In the steady-state regions, the proposed sensor can measure angle q₁ within 0.30% of its reference encoder value. However, in the transient region, the error causes an error spike of approximately 5% in the angle q₁. This is primarily attributed to amplification as this error is carried through the calculations for q₁.

[0130] Also note that no such noise spikes exist in the angular speed error plot. However, there is a distinct change in the error profile after the step. As described above, the magnetic sensor output for angular speed exhibits a periodic error at the operating frequency that results from variations at the direction cosine peaks. With the numerical differentiation, this noise is amplified or scaled by the rotation speed; consequently, the noise is proportional to the angular speed, which is apparent in FIG. 20. Of course, their relative error with respect to the nominal or reference speeds are similar. As a result, the proposed scheme can measure ∥ω∥ to within 0.50% of its reference encoder value for speeds up to 0.5 Hz.

[0131] 6. Summary of Results

[0132] A novel three-dimensional, non-contact angular velocity sensor is presented hereinabove. The sensor was developed to track the rotation of the ball wheel mechanism, but can be used in any application involving rigid-body rotation about a fixed point, or spherical motion. First, a non-contact motion tracking scheme based on magnetometry was introduced. The field magnetic field measurements are used to determine the absolute orientation of the magnet axis, which provides the absolute orientation of a vector fixed relative to the sphere. Next, an approach based on the natural invariants of rigid-body transformations is used to solve the inverse kinematics problem of determining the angular velocity vector of the sphere from the spherical-motion data.

[0133] Experimental results demonstrated that the sensing scheme can accurately track the rotation of a magnet spinning at steady-state speeds and at constant orientations of the rotation axis. The scheme also provides reasonable tracking capabilities during transients regimes, such as step changes in velocity or precession of the rotation axis. One possible approach to improving the transient tracking accuracy is a predictive scheme that utilizes the physical system dynamics to estimate the precession rate. Information on the precession rate, then, provides a means of extracting, or decoupling, the wheel rotation from the axis precession.

[0134] In summary, the proposed sensing scheme is capable of tracking the motion of a magnet rotating at a relatively slow angular rate without contact. While there remain issues related to packaging the sensor for use outside the laboratory, the experimental results show that the technology is viable for tracking the rotation of body about an arbitrary axis.

[0135] Although the description above contains many details, these should not be construed as limiting the scope of the invention but as merely providing illustrations of some of the presently preferred embodiments of this invention. Therefore, it will be appreciated that the scope of the present invention fully encompasses other embodiments which may become obvious to those skilled in the art. In the appended claims, reference to an element in the singular is not intended to mean “one and only one” unless explicitly so stated, but rather “one or more.” All structural, chemical, and functional equivalents to the elements of the above-described preferred embodiment that are known to those of ordinary skill in the art are expressly incorporated herein by reference and are intended to be encompassed by the present disclosure and claims. Moreover, it is not necessary for a device or method to address each and every problem sought to be solved by the present invention, for it to be encompassed by the present disclosure and claims. Furthermore, no element, component, or method step in the present disclosure is intended to be dedicated to the public regardless of whether the element, component, or method step is explicitly recited in the claims. No claim element herein is to be construed under the provisions of 35 U.S.C. 112, sixth paragraph, unless the element is expressly recited using the phrase “means for.”

[0136]

TABLE 1
Nomenclature
Symbol	Variable	units
{Br, Bt}	Radial-Tangential Components of Flux	W/m2
	Density Vector
{Bx, By, Bz}	Orthogonal Components of Flux Density	W/m2
	Vector
{circumflex over (p)}	Dipole Axis Unit Vector	unitless
{l, m, n}	Direction Cosines	unitless
{î, ĵ, {circumflex over (k)},}	Inertial Reference Frame	unitless
{er, en, et}	Rotating Reference Frame	unitless
r	Position Vector	m
r||	Component of Position Vector along	m
	Rotation Axis
r⊥	Projection of Position Vector Orthogonal	m
	to Rotation Axis
ê	Unit Vector Parallel to Rotation Axis	unitless
νp	Tangential Velocity Vector of Point p	m/s
δp	Displacement Vector of Point p	m
ω	Angular Velocity Vector	rad/s
k	Sample Index	unitless
T	Sample Period	s

[0137]

TABLE 2
Experimental Parameters
	Symbol	Variable	Value
	fs	Sampling Frequency	500 Hz
	ω	Input angular speed	0.25 Hz-0.50 Hz
	q1	Relative Orientation	0 rad-π/4 rad
	R	Distance between Magnet and	0.30 m(12 in)
		Sensor

[0138]

TABLE 3
APS 535 Specifications
	Sensor Range	0.02 to 5 Gauss
	Output Range	±10 V = ±5 Gauss
	Sensitivity	2 V/Gauss
	Linearity	±0.2%
	Frequency Response	DC to 400 Hz (−3 dB)