Patent Description:
Embodiments of the subject matter disclosed herein relate to navigation in planar robots.

Planar robots move on planes and are utilized in applications such as robot vacuum cleaners. Dead-reckoning navigation and simultaneous localization and mapping (SLAM) in planar robots utilize an inertial measurement unit (IMU), an optical flow (OF) sensor and wheel encoders (WE). The IMU, which typically contains an accelerometer and a gyroscope, provides the relative heading and linear acceleration of the planar robot. The OF sensor and WE provide the speed of the planar robot. Dead-reckoning is used to compute the trajectory of the planar robot by updating a current position estimate with the current speed and the previous position of the planar robot.

Although the OF sensor and WE can provide the speed of the planar robot, the measurements provided by the OF sensor and WE are prone to errors and failures. For example, an OF sensor can fail completely if the image quality of the OF is too low. In addition, wheel slips are a common cause of measurement errors for WE. For dead-reckoning navigation, accuracy of the speed estimates determines the position error growth rate; therefore, fusing sensors and obtaining accurate speed estimates are essential for navigating across heterogenous surfaces (e.g., rugs, tile, wood). Accordingly, there is still room for improvement in the navigation of planar robots.

The article entitled ""<NPL>, discloses using an extended Kalman filter approach and sensor fusion to determine errors in wheel encoder readings, according to prior art.

Patent publication <CIT>, discloses an apparatus and method for guiding a mobile platform within an environment, according to prior art.

Patent publication <CIT>, discloses a mobile body and a method and apparatus thereof, according to prior art.

Exemplary embodiments are directed to systems and methods that provide for navigation in planar robots. The linear acceleration from IMU and speeds from the OF sensor and WE are fused to achieve a robust and accurate dead-reckoning position estimate. Bad sensor measurements are rejected, or both OF sensor and WE measurements are combined properly with respect to their accuracies. Measurements from the OF sensor and WE may not be consistent with robot motion, and the errors in the OF sensor and WE depend on running conditions. However, the linear acceleration of the IMU is always consistent with robot motion. In addition, although the speed derived from the linear acceleration suffers from a quickly growing integration error, this derived speed becomes comparable with the speeds measured by the OF sensor and WE without motion inconsistency errors in a feature domain through signal processing. Moreover, the weight of sensor measurements can be derived with respect to their accuracies.

According to an embodiment there is a method for estimating a trajectory of a robot. The method comprising: fusing a plurality of robot velocity measurements from a plurality of robot sensors located within the robot to generate a fused robot velocity; and applying Kalman filtering to the fused robot velocity and measured linear acceleration from an inertial measurement unit to compute a current robot location. The method further comprises wherein fusing the plurality of robot velocity measurements comprises fusing an optical flow sensor velocity measurement and a wheel encoder sensor velocity measurement, and wherein fusing the optical flow sensor velocity measurement and the wheel encoder sensor velocity measurement comprises computing a weighted sum of the optical flow sensor velocity measurement and the wheel encoder sensor velocity measurement.

According to an embodiment there is a robot configured for estimating a trajectory of the robot. The robot comprising: at least one processor configured to fuse a plurality of robot velocity measurements from a plurality of robot sensors located within the robot to generate a fused robot velocity; and the at least one processor configured to apply Kalman filtering to the fused robot velocity and measured linear acceleration from an inertial measurement unit to compute a current robot location. The robot further comprises wherein to fuse the plurality of robot velocity measurements comprises the at least one processor fusing an optical flow sensor velocity measurement and a wheel encoder sensor velocity measurement, and wherein fusing the optical flow sensor velocity measurement and the wheel encoder sensor velocity measurement comprises the at least one processor computing a weighted sum of the optical flow sensor velocity measurement and the wheel encoder sensor velocity measurement.

The accompanying drawings illustrate exemplary embodiments, wherein:.

The following detailed description of the invention refers to the accompanying drawings. The same reference numbers in different drawings identify the same or similar elements. Also, the following detailed description does not limit the invention. Instead, the scope of the invention is defined by the appended claims.

As used herein, a variable displayed in bold text denotes a matrix or vector. Whether the variable is a matrix or vector can be inferred from the context. For a variable <MAT>, x stands for the quantity of interest, e.g., linear acceleration or velocity. The quantity A is the frame or coordinate in which x is, e.g., user frame or inertial measurement unit (IMU) frame, and the quantity b is the location with which x is associated, e.g., where the IMU or optical flow (OF) senor is located. The quantity c is the sensor by which x is measured, e.g., OF or wheel encoder (WE).

In general, the following symbols and variables are used throughout this specification:.

I  IMU body frame. i, o, w  Locations of the IMU, OF, and WE sensors on the robot. U  The user frame. ω  The angular velocity of the robot as [ <NUM><NUM>ωz]. <MAT>  The displacement from OF to IMU in the IMU frame. <MAT>  The displacement from WE to IMU in the IMU frame. <MAT>  The acceleration at the location of IMU in the user frame U, measured by IMU at time k. <MAT>  The velocities at the location of IMU in the frame U, measured by OF and W at time k. <MAT>  The true velocity at the location of IMU in the user frame U at time k. <MAT>  The fused velocity at the location of IMU in the user frame U at time K. <MAT>  The features computed from <MAT> and <MAT> in velocity domain. N(x; m, Φ )  The Gaussian distribution of x with mean m and covariance Φ.

Exemplary embodiments provide improved navigation of planar robots. Suitable planar robots include, but are not limited to, a robot vacuum cleaner (RVC). In general, a RVC system contains two wheels, a left wheel and a right wheel, each with their own encoders and driven by two independent motors. A description of how robotic vacuums work is found at https://electronics. howstuffworks. com/gadgets/home/robotic-vacuum1.

Referring initially to <FIG>, an embodiment of a robot <NUM> is illustrated. The robot includes a left wheel <NUM> containing a left wheel encoder (WE) sensor and a right wheel <NUM> containing a right wheel (WE) encoder sensor. The robot also includes an optical flow (OF) sensor <NUM> and an IMU <NUM>. The IMU <NUM> contains an accelerometer and a gyroscope and measures the acceleration <MAT> <NUM> at the location of the IMU, the angular velocity ωk <NUM>, and the angular position I→URk <NUM> or quaternion qk which is the rotation that rotates the IMU frame <NUM> to the user frame <NUM> or a fixed frame with z-axis <NUM> extending in the direction of gravity. For a planar robot, it is also the angular position of the robot. In <MAT> and I→URk, I and U stands for the IMU frame and the user frame. The left subscript i indicates the quantity is associated with the location of the IMU <NUM>, and the right superscript i indicates the quantity is measured by the IMU <NUM>. The discrete time is indicated by k.

The OF sensor <NUM> measures the pixel motion in the OF body frame, which is the relative distance that the sensor moves between two consecutive register readings. When sampled with a constant period, the OF sensor <NUM> can be treated as a velocity sensor. The OF sensor <NUM>, which is calibrated, measures the velocity <NUM> at the location of the OF sensor <NUM>, which is denoted as <MAT>. The displacement from the OF sensor <NUM> to the IMU <NUM> in the IMU frame is denoted <MAT>, which is constant in the IMU frame.

After calibration, the left and right WE sensors measure the velocities at the locations of the left wheel <NUM> and the right wheel <NUM> in the IMU frame, respectively, which are denoted as <MAT> and <MAT>. On a planar robot, these two wheels can only move forward or backward. The average of <MAT> and <MAT> is the robot velocity <NUM> at the middle w <NUM> between two wheel centers, which is denoted as <MAT>. The displacement <NUM> from the location w to the IMU <NUM> in the IMU frame is <MAT>. The robot <NUM> also includes at least one processor <NUM> for performing the various calculations and implementing instructions associated with the embodiments described herein.

An optical flow sensor measures changes in position by optically acquiring sequential surface images (frames) and mathematically determining the speed and the direction of motion, employing a low resolution, high frame rate camera to capture images of a surface. Consecutive images are fed to an image processing algorithm that produces delta X and Y, the change in position during the intervening interval. A familiar application of an optical flow sensor is in a computer mouse, where the image sensor and processing algorithm are incorporated into a single chip.

The accuracy of the OF measurements are light source and surface texture dependent. A suitable OF sensor <NUM> for some embodiments is the PixArt Imaging PAA <NUM> that has both light amplification by stimulated emission of radiation (LASER) and light emitting diode (LED) light sources. When LASER is chosen for illumination, the Image Quality value is high when tracking on tiles and is low when tracking on carpets. On the contrary, when LED is chosen, the Image Quality is high when tracking on carpets and is low when tracking on tiles. Because the sensor operates by measuring optical flow, surfaces which cause the patterns of light intensity to behave differently will result in inaccurate measurements. Therefore, specular reflection from the surface will impact the measurements as described in <NPL>) and "<NPL>. In order to provide for the fusion of measurements from different sensors, the failures of the OF sensor need to be reliably detected, and the accuracy of the OF sensor needs to be assessed when the OF sensor is running on different surfaces.

Regarding the accuracy of the WE, a robot is usually equipped with wheel odometry to measure its moving distance. The accuracy of the WE measurements are terrain surface dependent and are subject to many sources of errors as described in "<NPL>). These sources of errors include limited resolution during integration (time increments, measurement resolution, etc.), misalignment of the wheels (deterministic), unequal wheel diameter (deterministic), variation in the contact point of the wheel, unequal floor contact (slipping, non-planar surface, etc.), and robot collision and bump induced error. These errors adversely affect the reliability of the WE sensor measurements. These deterministic errors can be significantly reduced by dynamic wheel encoder calibrations. For the other errors, accuracy needs to be estimated for sensor fusion.

An IMU <NUM> provides the orientation and linear acceleration of the robot. The heading changes of a robot can be derived from the orientation. The linear acceleration usually contains the zero-gravity-offset (ZGO) and gravity residuals. Consequently, the direct integration of the linear acceleration suffers from severe integration errors. However, the accuracy of the IMU's linear acceleration is not subject to variations in terrains and surface textures. In other words, the linear acceleration measurements are always consistent with robot motion. After filtering and signal processing, the linear acceleration derived from the IMU <NUM> is used to detect OF and WE sensor failures and assess their accuracies.

Referring to <FIG>, an exemplary embodiment of a method for estimating the current position of a robot <NUM> with good accuracy is illustrated. The method fuses the velocity measurements from a plurality of robot sensors and uses the fused velocity in a Kalman filter to estimate the current position. When performed over time, the current positions define the robot trajectory. Suitable robots include, but are not limited to, planar robots. In one embodiment, the robot is an RVC. Initially, each robot sensor in the plurality of robot sensors is calibrated. In one embodiment, a given robot sensor is calibrated at step <NUM> using nominal calibration methods provided by the original equipment manufacturer (OEM) of that given robot sensor. Alternatively, a given robot sensor is calibrated using an in-house advanced calibration method. Suitable robot sensors include OF sensors and WE sensors as described above. The calibrated robot sensors are time aligned at step <NUM>. A robot velocity measurement is obtained from each robot sensor at step <NUM>. In one embodiment, an OF sensor velocity measurement is obtained, and at least one WE sensor velocity measurement is obtained. In one embodiment, two separate WE sensor velocity measurements are obtained, and the two separate WE sensor velocity measurements are averaged to generate a composite WE sensor velocity measurement. The composite WE sensor velocity measurement is associated with the center of the axes between the left and right wheels, most of, which is also a center of the body of the robot.

The robot velocity measurements from the plurality of robot sensors are then fused or combined at step <NUM>. In one embodiment, the combination is a weighted combination or weighted sum of the plurality of robot velocity measurements. The fused robot velocity measurements are then processed by a Kalman filter at step <NUM>, i.e., subjected to Kalman filtering, to estimate a current robot location at step <NUM>. Collection and fusion of the robot velocity measurements and processing of the fused velocity measurements with Kalman filtering can be repeated iteratively. This generates a history of current robot locations that can be used to determine a robot trajectory at step <NUM> over a given time period. In one embodiment the fused robot velocity measurements are processed by the Kalman filter in combination with the linear acceleration obtained from the IMU <NUM> at step <NUM> and expressed in the user frame of reference and robot dynamics at step <NUM>.

Since each robot sensor measures quantities at the physical location of that robot sensor and the IMU <NUM> measures the linear acceleration at the location of the IMU, in one embodiment, the robot velocity measurements, e.g., the OF sensor velocity measurement and the WE sensor velocity measurements, are converted, transformed or translated from the robot velocity measurements at the location of each robot sensor to robot velocity measurements at a common location within the robot at step <NUM>. In one embodiment, the robot velocity measurements are converted to an arbitrary common location. In one embodiment, each robot velocity measurement is converted from the location of each robot sensor to a corresponding robot velocity measurement at the location of the IMU within the robot. Therefore, each robot velocity measurement is translated from a robot sensor location frame of reference to the IMU location frame of reference.

Different conversion choices for converting the robot velocity measurements will introduce different conversion errors. In one embodiment, all quantities from the robot velocity measurements are converted to the location of IMU, i.e., the OF sensor velocity measurement and the WE sensors velocity measurements are converted to the location of IMU. On a rigid body, the velocity at a second location, b, is computed from the velocity at a first location, a, using the rigid body angular velocity and displacement from a to b. This is expressed in the following rigid body conversion equation (<NUM>): <MAT> where <MAT> and <MAT> are the velocities at the location b and location a, respectively, ω is the angular velocity of the rigid body, and <MAT> is the displacement from a to b in the body frame. The velocity conversion equation holds in the user frame.

In one embodiment, the robot velocity measurements, i.e., the OF sensor velocity measurement and the WE sensors velocity measurements are converted to the location of the IMU using the rigid body conversion equation. The corresponding velocities at IMU computed from the OF sensor and WE sensors are shown in equations (<NUM>) and (<NUM>): <MAT> <MAT> where <MAT> and <MAT> are the velocities from the OF sensor and WE at their own locations in the IMU frame, respectively, and <MAT> and <MAT> are the corresponding velocities at the location of IMU.

The resulting robot velocity measurements in the IMU frame of reference are then converted to corresponding robot velocity measurements in a user frame of reference at step <NUM>. The user frame of reference is a frame of reference associated with a user of the robot. In one embodiment, the quaternion or rotation matrix from the IMU provides the rotation from IMU frame to the user frame or a fixed coordinate frame. With such a quaternion q, the velocities <MAT> and <MAT> at the location of IMU is converted into the following user coordinates as shown in equations (<NUM>) and (<NUM>): <MAT> <MAT>.

In addition, measurements from the IMU <NUM> are translated to the user frame of reference. In one embodiment, the linear acceleration from the IMU <NUM> in the user frame of reference is computed as follows as shown in equation (<NUM>): <MAT>.

The accuracy of sensor measurements from the plurality of robot sensors, e.g., OF sensors and WE sensors, are dependent upon the running or operating conditions of the robot <NUM>. The accuracy of one type of robot sensor under given running or operating conditions can be different than another type of robot sensor under the same running or operating conditions. Therefore, certain types of robot sensors will have greater accuracy than other types of robot sensors under common operating conditions. Therefore, the accuracy of each sensor measurement from each robot sensor is determined based on the running conditions at step <NUM>, and this accuracy is used in determining how to combine or to fuse the robot velocity measurements for use in Kalman filtering.

Given that IMU measurements are immune or agnostic to the running conditions of robot, in one embodiment, IMU linear acceleration measurements in a feature space are used to access or to determine the accuracies of the plurality of robot velocity measurements reliably. Weights to be applied to the robot velocity measurements from each robot sensor, the OF sensor and the WE sensors, are computed with respect to the accuracies associated with those velocity measurements. In addition to the robot velocity measurements obtained from the robot sensors, a robot velocity is derived from the linear acceleration of the robot that is obtained from the IMU. Features are defined for the purpose of comparing the velocities obtained from the robot sensors to velocities calculated from the IMU derived linear acceleration. According to some embodiments, a feature is generated by a DC-Block filter which transforms a velocity signal, wherein the velocity signal is generated by one of the optical flow sensor, the wheel encoder or the inertial measurement unit.

Comparable features from the linear acceleration derived robot velocity and the robot velocity measurements from the plurality of robot sensors are computed and compared for different domains, e.g., a velocity domain and a position domain at step <NUM>. Differences between the comparable features are identified between the IMU and each robot sensor at step <NUM>. These differences express the accuracy of the velocity measurements from each robot sensor and are used to weight the contribution of each robot velocity measurement in the fusion of velocity measurements at step <NUM>. Therefore, fusing the robot velocity measurements uses a weighted sum of the robot velocity measurements of each robot sensor based on the accuracy of each robot sensor under current running or operating conditions of the robot.

Since the linear acceleration from the IMU is immune to errors induced by the running conditions that adversely affect the OF sensor and WE sensors, the linear acceleration is used to assess the accuracies of <MAT> and <MAT>. Expressing the true velocity at the location of the IMU as <MAT> at instant k, yields the following relationships between the velocities obtained from the OF sensor and WE sensors and the true velocity as shown in equation (<NUM>): <MAT> where <MAT> and <MAT> represent the sensor noises. Moreover, if <MAT> is ideal without errors, the velocity derived from the linear acceleration is expressed by the following equation (<NUM>): <MAT> The calculated velocity, <MAT>, is close to the true velocity, <MAT>. Thus, <MAT>and <MAT>, with ≅ expressing the effect of sensor errors. Stated another way, the velocity <MAT> derived from the linear acceleration <MAT>, and <MAT> are comparable or similar without running condition induced errors. The same goes for <MAT> and <MAT>. However, <MAT> usually contains ZGO and gravity residuals. Integration of <MAT> accumulates these errors quickly, making the calculated velocity far different from the true velocity and invalidating the above assumptions.

To overcome the integration divergence due to the ZGO and gravity residual contained in the acceleration, a DC-block filter is applied before and after each integration operation so that only the signal variation is kept. A DC-block filter is a recursive filter specified by the difference equation (<NUM>): <MAT>.

Referring to <FIG>, a graph of frequency responses <NUM> when a = <NUM><NUM>, a = <NUM><NUM>, and a = <NUM><NUM> is illustrated.

To obtain the comparable features between the velocity derived from the IMU linear acceleration, the OF sensor velocity measurement and the WE sensor velocity measurement, the DC-block filter and appropriate conversions are applied to the velocity measurements of the OF sensor, the WE sensors and the IMU. This converts the sensor measurements into transformed signals in a transform domain. The transformed signal is referred to as a feature of the original signal, i.e., the original velocity measurement. The transformed signals, i.e., features, are comparable, and the differences between the features are used to assess sensor measurements accuracy. In one embodiment, the sensor measurements are converted for multiple transform domains, e.g., velocity domain and position domain.

A well-designed DC-block filter fDCB(*) is used to compute the features of the original signals, i.e., the original sensor measurements, in the velocity domain and position domain that are used to compare the IMU linear acceleration derived velocity with each robot velocity measurement from each robot sensor. First, the DC-blocked filter is applied to the linear velocity <MAT> that is derived from the IMU linear acceleration <MAT>. The resulting feature is shown in equation (<NUM>): <MAT>.

The innermost fDCB(*) removes the DC component in <MAT>, e.g., gravity residual. The outside fDCB(*) removes the accumulated error due to the integration. The left subscript v in <MAT> indicates the feature is in velocity domain.

The same DC-block filter is applied to <MAT> and <MAT>, yielding equation (<NUM>): <MAT>.

After applying the double DC-block filter, the DC component and the first order variation component embedded in the original signal are filtered out, and only the higher order signal variations are kept. The error caused by the ZGO and gravity residual are reduced significantly. The following relations hold for real data as shown in equations (<NUM>) and (<NUM>): <MAT> and <MAT>.

These relations bridge the gap between theoretical formulation and practical implementation. To further reduce high frequency sensor noise, the velocities are also converted in the position domain and the DC-block filter is applied as shown in equations (<NUM>) - (<NUM>): <MAT> <MAT> <MAT> Then, as shown in equations (<NUM>) and (<NUM>): <MAT> and <MAT> The left subscript p in <MAT> indicates the feature is in position domain.

In assessing the feature differences, a correlation coefficient between the computed features is calculated and is used to determine the closeness or similarity between the IMU measurements and each robot sensor measurement, i.e., between OF measurements and IMU measurements and between WE measurements and IMU measurements. The similarity metrics are computed between the k<NUM> + <NUM> feature samples within the most recent t<NUM> seconds as shown in equations (<NUM>) and (<NUM>): <MAT> <MAT> where <MAT> is the sample sequence of feature pΓIMU from (k - k<NUM>) to k reshaped as a vector, and <MAT> is the sample sequence of feature pΓOF from (k - k<NUM>) to k reshaped as a vector.

When λio is below a threshold, e.g., <NUM>, OF sensor measurements suffer from running condition errors, e.g. image glitches, and when λio is high enough or beyond a threshold, e.g., <NUM>, OF sensor measurements have acceptable accuracy. Similarly, when λiw is low enough below a threshold, e.g., <NUM>, WE measurements suffer from running condition errors, and when λiw is high enough beyond a threshold, e.g., <NUM>, WE measurements have acceptable accuracy. Therefore, when λio is high enough or beyond a threshold, e.g., <NUM>, while λiw is low enough or below a threshold, e.g., <NUM>, the fused velocity is <MAT>. That is, the weight assigned to the WE sensor velocity measurement is zero, and the weight assigned to the OF sensor velocity measurement is <NUM>. When λiw is high enough or beyond a threshold, e.g., <NUM>, while λio is low enough or below a threshold, e.g., <NUM>, the fused velocity should be <MAT>. That is, the weight assigned to the OF sensor velocity measurement is zero, and the weight assigned to the WE sensor velocity measurement is <NUM>. Otherwise, the fused velocity is calculated as a weighted sum of <MAT> and <MAT> with the weights used in the weighted sum determined by the following procedure.

To derive the weights of the velocities associated with the OF sensor and the WE sensors, the linear acceleration <MAT> is assumed to be accurate without ZGO or gravity residual, and <MAT> and <MAT> express only sensor measurements. Therefore, as shown in equations (<NUM>) and (<NUM>), <MAT> <MAT>.

Assume <MAT> and <MAT> are the measurement noises and gaussian distributed with distributions of <MAT> and <MAT>, respectively. The likelihood of <MAT> given <MAT> is defined as <MAT>, and the likelihood of <MAT> given <MAT> is defined as <MAT>. With the initial probabilities of <MAT>, the weights by Bayesian formula are shown in equations (<NUM>) and (<NUM>): <MAT> <MAT> Then the fused velocity is shown in equation (<NUM>): <MAT>.

Given the initial robot velocity <MAT> and the linear acceleration sequence of <MAT>, the likelihood of <MAT> and <MAT>, and <MAT>, are computed. However, due to the ZGO and gravity residual, the formulas (<NUM>) and (<NUM>) are not valid for real data. For example, an IMU <NUM> with <NUM> milligravity offset error causes about <NUM>/s error for the speed after one second and about <NUM>/s error for the speed after <NUM> seconds. Therefore, two embodiments are used to approximate the likelihoods.

In one embodiment, the likelihood <MAT> is approximated by the most recent N+<NUM> samples, which are obtained within most recent <NUM> or <NUM> seconds, of the linear acceleration samples <MAT> and the Nth previous measurement <MAT> as shown below in equation (<NUM>): <MAT>.

Since in a short period of time, the following approximation holds even with gravity residual and ZGO, as shown in equation (<NUM>): <MAT> the OF velocity residual is computed from as shown in equation (<NUM>): <MAT> Assuming that the OF sensor has Gaussian noise with distribution of <MAT>, then the likelihood of the residual <MAT> is <MAT> given <MAT> and <MAT>.

Similarly, the WE velocity residual is computed as shown in equation (<NUM>): <MAT> Its likelihood is <MAT> given <MAT> and <MAT>. Therefore, as shown in equations (<NUM>) and (<NUM>): <MAT> <MAT> Both likelihood functions mainly depend on the common data <MAT>, while <MAT> and <MAT> are assumed approximately equal or their difference is ignored.

In another embodiment, features in the velocity domain are used to compute the weights. As defined in equations (<NUM>)-(<NUM>): <MAT> <MAT> <MAT>.

If both the OF sensor and the WE sensor work properly, <MAT> and <MAT> <MAT>. It is known that <MAT> where <MAT> is the true velocity at IMU in the user frame. Since, as shown in equation (<NUM>), <MAT> where <MAT> and <MAT> are the sensor noises, then, as shown in equation (<NUM>), <MAT>.

Assuming <MAT> and <MAT> are Gaussian distributed with distributions of <MAT> and <MAT>, respectively, given <MAT>, the likelihood of <MAT> is <MAT>, and the likelihood of <MAT> is <MAT>. Therefore, as shown in equation (<NUM>): <MAT> <MAT>.

In this embodiment, the likelihood is approximately by defining <MAT> <MAT> and <MAT>, where <MAT> and <MAT> are the most recent N+<NUM> features, which are the samples within the most recent <NUM> or <NUM> seconds. The most recent N+<NUM> samples are used to make assessment more stable. Then the weights are shown below in equations (<NUM>) and (<NUM>): <MAT> <MAT> Therefore, the fused robot velocity is <MAT>.

Having fused the velocities of the OF sensor and WE, the robot position is estimated. For the dynamics of the robot, the robot is assumed to be running at constant acceleration. Its state is xk = [xk, ẋk, ẍk, yk, ẏk, ÿk], where xk and yk are the position at instant k.

The dynamics of the robot are given by xk= Fxk-<NUM>+wk and zk= Hxk+nk, where Fand H are shown below in equations (<NUM>) and (<NUM>), respectively: <MAT> <MAT> and wk and nk are the dynamic process noise and measurement noise. It is assumed that wk is of Gaussian distribution with zero mean and covariance of Qk and that nk is of Gaussian distribution with zero mean and covariance of Rk. T is the sampling period.

The measurements are <MAT>. The Kalman filter is applied to estimate position of the robot as shown in equations (<NUM>)-(<NUM>): <MAT> <MAT> <MAT> <MAT> <MAT> <MAT> The computed robot position is [x̂ k|k[<NUM>] x̂ k|k[<NUM>]].

The velocity fusion and position estimation described herein are summarized in <FIG>. Further, <FIG> can be mapped to <FIG> for many of the steps, i.e., in some steps, <FIG> shows additionally details, for example, various equations described herein and specific inputs, for certain steps described in <FIG>. Therefore, the interested reader is encouraged to compare <FIG> with <FIG> for ease of understanding.

Initially, flowchart <NUM> includes inputs from the optical flow <NUM>, the IMU <NUM> and the wheel encoder <NUM>. Blocks <NUM> and <NUM> represent processing the various inputs shown to provide velocity outputs associated with the WE <NUM> and the OF <NUM>, respectively. The velocity outputs from blocks <NUM> and <NUM> are then merged with information from the IMU <NUM> as shown in step <NUM> described as rotate to user frame by R. Integrate to velocity occurs in step <NUM>. Then the DC-block filter is applied before and after each integration operation so that only the signal variation is kept as shown in step <NUM>. Computing feature difference occurs in step <NUM>, followed by the computing weights in step <NUM>. Fusion of the velocities of the WE <NUM> and OF <NUM> occurs in step <NUM>. Block <NUM> represents inputting the robot dynamics and linear acceleration, with the output being sent to the Kalman filter for filtering as shown in step <NUM>.

According to an embodiment, there is a flowchart <NUM> of a method for estimating a trajectory of a robot as shown in <FIG>. The method includes: in step <NUM>, fusing a plurality of robot velocity measurements from a plurality of robot sensors located within a robot to generate a fused robot velocity; and in step <NUM>, applying Kalman filtering to the fused robot velocity and measured linear acceleration from an inertial measurement unit to compute a current robot location.

Claim 1:
A method (<NUM>) for estimating a trajectory (<NUM>) of a robot (<NUM>, <NUM>), the method comprising:
fusing (<NUM>, <NUM>) a plurality of robot velocity measurements from a plurality of robot sensors (<NUM>, <NUM>) located within the robot (<NUM>, <NUM>) to generate a fused robot velocity; and
applying Kalman filtering (<NUM>, <NUM>) to the fused robot velocity and a measured linear acceleration (<NUM>) from an inertial measurement unit (<NUM>) to compute a current robot location (<NUM>),
wherein fusing (<NUM>) the plurality of robot velocity measurements comprises fusing an optical flow sensor velocity measurement and a wheel encoder sensor velocity measurement,
characterized in that
fusing (<NUM>) the optical flow sensor velocity measurement and the wheel encoder sensor velocity measurement comprises computing (<NUM>) a weighted sum of the optical flow sensor velocity measurement and the wheel encoder sensor velocity measurement.