LOCALIZATION METHOD FOR MOBILE ROBOTS BASED ON LANDMARKS

0Associated
Cases 
0Associated
Defendants 
0Accused
Products 
36Forward
Citations 
0
Petitions 
2
Assignments
First Claim
1. A method for estimating the location and orientation of a mobile robot with respect to landmarks whose positions are given, the robot provided with a sensor for measuring bearings of the landmarks with respect to the robot, where bias errors are present, the method comprising:
 using the sensor to obtain bearings to the landmarks;
estimating the location and orientation of the robot with respect to the landmarks, based on the measured bearings, correcting bias errors associated with the measured bearings to obtain corrected bearings by finding a correction function that minimizes the sum of squared errors between measured bearings and the corrected bearings; and
determining the location and the orientation of the robot to be where the sum of squared errors between measured bearings and the corrected bearings is minimal.
2 Assignments
0 Petitions
Accused Products
Abstract
A method for estimating the location and orientation of a mobile robot with respect to landmarks whose positions are given using a sensor for measuring bearings of the landmarks with respect to the robot, where bias errors are present. The method comprises using the sensor to obtain bearings the landmarks. The location and orientation of the robot with respect to the landmarks is estimated, based on the measured bearings, and bias errors associated with the measured bearings are corrected to obtain corrected bearings. A correction function is used to finds the minimal sum of squared errors between measured bearings and the corrected bearings. The location and the orientation of the robot is determined to be where the sum of squared errors between measured bearings and the corrected bearings is minimal.
37 Citations
View as Search Results
Method and apparatus for simulating a physical environment to facilitate vehicle operation and task completion  
Patent #
US 20110216185A1
Filed 03/02/2010

Current Assignee
Crown Equipment Corporation

Sponsoring Entity
Crown Equipment Corporation

METHOD AND APPARATUS FOR SENSING OBJECT LOAD ENGAGEMENT, TRANSPORTATION AND DISENGAGEMENT BY AUTOMATED VEHICLES  
Patent #
US 20110218670A1
Filed 03/05/2010

Current Assignee
Crown Equipment Corporation

Sponsoring Entity
Crown Equipment Corporation

DEEP LANE NAVIGATION SYSTEM FOR AUTOMATIC GUIDED VEHICLES  
Patent #
US 20120330492A1
Filed 05/31/2012

Current Assignee
John Bean Technologies Corporation

Sponsoring Entity
John Bean Technologies Corporation

Method and apparatus for simulating a physical environment to facilitate vehicle operation and task completion  
Patent #
US 8,508,590 B2
Filed 03/02/2010

Current Assignee
Crown Equipment Corporation

Sponsoring Entity
Crown Equipment Corporation

Method and apparatus for sensing object load engagement, transportation and disengagement by automated vehicles  
Patent #
US 8,538,577 B2
Filed 03/05/2010

Current Assignee
Crown Equipment Corporation

Sponsoring Entity
Crown Equipment Corporation

Method and apparatus for automatically calibrating vehicle parameters  
Patent #
US 8,548,671 B2
Filed 06/06/2011

Current Assignee
Crown Equipment Corporation

Sponsoring Entity
Crown Equipment Corporation

Method and apparatus for facilitating map data processing for industrial vehicle navigation  
Patent #
US 8,589,012 B2
Filed 06/14/2011

Current Assignee
Crown Equipment Corporation

Sponsoring Entity
Crown Equipment Corporation

Method and apparatus for sharing map data associated with automated industrial vehicles  
Patent #
US 8,594,923 B2
Filed 06/14/2011

Current Assignee
Crown Equipment Corporation

Sponsoring Entity
Crown Equipment Corporation

Method and apparatus for providing accurate localization for an industrial vehicle  
Patent #
US 8,655,588 B2
Filed 11/18/2011

Current Assignee
Crown Equipment Corporation

Sponsoring Entity
Crown Equipment Corporation

METHOD AND APPARATUS FOR USING UNIQUE LANDMARKS TO LOCATE INDUSTRIAL VEHICLES AT STARTUP  
Patent #
US 20140058612A1
Filed 11/14/2013

Current Assignee
Crown Equipment Corporation

Sponsoring Entity
Crown Equipment Corporation

MOBILE ROBOT SYSTEM AND METHOD OF CONTROLLING THE SAME  
Patent #
US 20140156071A1
Filed 02/10/2014

Current Assignee
Samsung Electronics Co. Ltd.

Sponsoring Entity
Samsung Electronics Co. Ltd.

Deep lane navigation system for automatic guided vehicles  
Patent #
US 9,046,893 B2
Filed 05/31/2012

Current Assignee
John Bean Technologies Corporation

Sponsoring Entity
John Bean Technologies Corporation

Method and apparatus for using prepositioned objects to localize an industrial vehicle  
Patent #
US 9,056,754 B2
Filed 11/08/2012

Current Assignee
Crown Equipment Corporation

Sponsoring Entity
Crown Equipment Corporation

Associating parking areas with destinations  
Patent #
US 9,151,628 B1
Filed 01/30/2015

Current Assignee
Nissan Motor Co. Ltd.

Sponsoring Entity
Nissan Motor Co. Ltd.

Method and apparatus for efficient scheduling for multiple automated nonholonomic vehicles using a coordinated path planner  
Patent #
US 9,188,982 B2
Filed 04/10/2012

Current Assignee
Crown Equipment Corporation

Sponsoring Entity
Crown Equipment Corporation

Method and apparatus for using unique landmarks to locate industrial vehicles at startup  
Patent #
US 9,206,023 B2
Filed 11/14/2013

Current Assignee
Crown Equipment Corporation

Sponsoring Entity
Crown Equipment Corporation

Mobile robot system and method of controlling the same  
Patent #
US 9,314,925 B2
Filed 02/10/2014

Current Assignee
Samsung Electronics Co. Ltd.

Sponsoring Entity
Samsung Electronics Co. Ltd.

ASSOCIATING PASSENGER DOCKING LOCATIONS WITH DESTINATIONS  
Patent #
US 20160209848A1
Filed 01/15/2015

Current Assignee
Nissan Motor Co. Ltd.

Sponsoring Entity
Nissan Motor Co. Ltd.

Associating passenger docking locations with destinations using vehicle transportation network partitioning  
Patent #
US 9,436,183 B2
Filed 01/15/2015

Current Assignee
Nissan Motor Co. Ltd.

Sponsoring Entity
Nissan Motor Co. Ltd.

Autonomous vehicle routing and navigation using passenger docking locations  
Patent #
US 9,448,559 B2
Filed 01/15/2015

Current Assignee
Nissan Motor Co. Ltd.

Sponsoring Entity
Nissan Motor Co. Ltd.

Autonomous robot localization  
Patent #
US 9,510,505 B2
Filed 10/10/2014

Current Assignee
iRobot Corporation

Sponsoring Entity
iRobot Corporation

Associating passenger docking locations with destinations  
Patent #
US 9,519,290 B2
Filed 01/15/2015

Current Assignee
Nissan Motor Co. Ltd.

Sponsoring Entity
Nissan Motor Co. Ltd.

Marker aided autonomous vehicle localization  
Patent #
US 9,541,409 B2
Filed 12/18/2014

Current Assignee
Nissan Motor Co. Ltd.

Sponsoring Entity
Nissan Motor Co. Ltd.

Method for positioning a tobepositioned device  
Patent #
US 9,568,586 B2
Filed 03/07/2016

Current Assignee
National Chiao Tung University

Sponsoring Entity
National Chiao Tung University

Associating parking areas with destinations based on automatically identified associations between vehicle operating information and nonvehicle operating information  
Patent #
US 9,568,335 B2
Filed 01/30/2015

Current Assignee
Nissan Motor Co. Ltd.

Sponsoring Entity
Nissan Motor Co. Ltd.

Method and apparatus for using unique landmarks to locate industrial vehicles at startup  
Patent #
US 9,580,285 B2
Filed 04/17/2014

Current Assignee
Crown Equipment Corporation

Sponsoring Entity
Crown Equipment Corporation

Passenger docking location selection  
Patent #
US 9,625,906 B2
Filed 01/15/2015

Current Assignee
Nissan Motor Co. Ltd.

Sponsoring Entity
Nissan Motor Co. Ltd.

Spatial clustering of vehicle probe data  
Patent #
US 9,697,730 B2
Filed 03/13/2015

Current Assignee
Nissan Motor Co. Ltd.

Sponsoring Entity
Nissan Motor Co. Ltd.

Pattern detection using probe data  
Patent #
US 9,778,658 B2
Filed 03/13/2015

Current Assignee
Nissan Motor Co. Ltd.

Sponsoring Entity
Nissan Motor Co. Ltd.

Automated guided vehicle system  
Patent #
US 9,864,371 B2
Filed 03/20/2015

Current Assignee
John Bean Technologies Corporation

Sponsoring Entity
John Bean Technologies Corporation

Routing of automated guided vehicles  
Patent #
US 9,886,036 B2
Filed 02/10/2015

Current Assignee
John Bean Technologies Corporation

Sponsoring Entity
John Bean Technologies Corporation

System for efficient scheduling for multiple automated nonholonomic vehicles using a coordinated path planner  
Patent #
US 9,958,873 B2
Filed 10/13/2015

Current Assignee
Crown Equipment Corporation

Sponsoring Entity
Crown Equipment Corporation

Autonomous vehicle routing and navigation using passenger docking locations  
Patent #
US 10,012,995 B2
Filed 09/19/2016

Current Assignee
Nissan Motor Co. Ltd.

Sponsoring Entity
Nissan Motor Co. Ltd.

Autonomous robot localization  
Patent #
US 10,067,232 B2
Filed 12/05/2016

Current Assignee
iRobot Corporation

Sponsoring Entity
iRobot Corporation

Identifying significant locations based on vehicle probe data  
Patent #
US 10,120,381 B2
Filed 12/21/2015

Current Assignee
Nissan Motor Co. Ltd.

Sponsoring Entity
Nissan Motor Co. Ltd.

Automatically identifying associations between vehicle operating data and nonvehicle operating data  
Patent #
US 10,269,240 B2
Filed 02/08/2017

Current Assignee
Nissan North America Incorporated

Sponsoring Entity
Nissan North America Incorporated

Computer peripheral floor cleaning system and navigation method  
Patent #
US 5,995,884 A
Filed 03/07/1997

Current Assignee
Timothy P. Allen, James Pauline O.

Sponsoring Entity
Timothy P. Allen, James Pauline O.

12 Claims
 1. A method for estimating the location and orientation of a mobile robot with respect to landmarks whose positions are given, the robot provided with a sensor for measuring bearings of the landmarks with respect to the robot, where bias errors are present, the method comprising:
 using the sensor to obtain bearings to the landmarks;
estimating the location and orientation of the robot with respect to the landmarks, based on the measured bearings, correcting bias errors associated with the measured bearings to obtain corrected bearings by finding a correction function that minimizes the sum of squared errors between measured bearings and the corrected bearings; and
determining the location and the orientation of the robot to be where the sum of squared errors between measured bearings and the corrected bearings is minimal.  View Dependent Claims (2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12)
 using the sensor to obtain bearings to the landmarks;
1 Specification
The present invention relates to navigation of mobile devices. More specifically, the present invention relates to a localization method for mobile robots based on landmarks.
BACKGROUND OF THE INVENTIONMobile robots, including Automated Guided Vehicles (AGVS), are becoming more and more common in society. Mobile robots are widely used in industry, military and security environments. They also appear as consumer products, such as, but not limited to, for entertainment, as toy robots for instance, or to perform certain tasks like vacuum cleaning for instance.
Localization techniques refer to processes by which an Automated Guided Vehicles and mobile robots robot determines its position with respect to its surroundings in the workspace. This is an essential component for path planning to specific locations and for preventing the robot from reaching undesired locations.
While Global Positioning Systems (GPS) provide comfortable solutions for outdoor localization, other methods have to be considered for indoor localization.
One of the methods to perform indoor localization is to put landmarks at known locations and measure their bearings with respect to the robot during robot operation.
The traditional geometric method for localization from landmark bearings is based on the idea that two bearing measurements enforce the position of the robot to lie on a defined circle.
Thus, according to various investigators among them are M. Betke, and L. Gurvits “Mobile robot Localization using landmarks” in IEEE Trans. Robot. Automat., Vol. 13, number 2, pages 251263, 1997, when three measurements are available the robot location can be computed.
The problem associated with this method is that it does not scale up for more than three measurements.
To overcome this problem, I. Shimshoni “On Mobile Robot Localization from Landmark Bearings” in IEEE Trans. Robotics and Automation volume 18 number 3, pages 971976, 2002 suggested an efficient algebraic technique which allows estimating the error when more than three measurements are available.
Such algebraic technique deals correctly with unbiased noise and yields close to optimal results. However, in real world situations other sources of errors are present and must be handled by a localization method.
Thus, since mobile robot systems are widely used, an aim of the present invention is to disclose a localization method which enables handling a wide range of error sources reliably and efficiently.
Other advantages and aims of the present invention will become apparent after reading the present invention and reviewing the accompanying figures.
SUMMARY OF THE INVENTIONThere is thus provided according to embodiments of the present invention, a method for estimating the location and orientation of a mobile robot with respect to landmarks whose positions are given, the robot is provided with a sensor for measuring bearings of the landmarks with respect to the robot where bias errors are present, the method comprising:
using the sensor to obtain bearings to the landmarks;
estimating the location and orientation of the robot with respect to the landmarks based on the measured bearings;
correcting bias errors associated with the measured bearings to obtain corrected bearings by finding a correction function that minimizes the sum of squared errors between measured bearings and the corrected bearings; and
determining the location and the orientation of the robot to be where the sum of squared errors between measured bearings and the corrected bearings is minimal.
Furthermore, in accordance with some embodiments of the present invention, the correction function relates to parameters which include calculated angle of bearing βc<sub>i</sub>, for each of the landmarks, denoted i, angle of obtained bearing β<sub>i</sub>, for each of the landmarks, position of each of the landmark, Pi<sub>x,y</sub>, position of the robot, P<sub>x,y </sub>and estimated orientation angle of the robot, θ.
The correction function further comprising estimating an angular error value for each of the landmarks calculated to be Δβi=(βc<sub>i</sub>−θ)−βi and calculating an error value associated with the determined location and the orientation of the robot to be:
<maths id="MATHUS00001" num="00001"><math overflow="scroll"><mrow><mi>S</mi><mo>=</mo><mrow><munderover><mo>∑</mo><mrow><mi>i</mi><mo>=</mo><mn>1</mn></mrow><mi>n</mi></munderover><mo></mo><mfrac><mrow><mi>Δ</mi><mo></mo><mstyle><mspace width="0.3em" height="0.3ex"/></mstyle><mo></mo><msubsup><mi>β</mi><mi>i</mi><mn>2</mn></msubsup></mrow><mi>n</mi></mfrac></mrow></mrow></math></maths>
where n is the number of the landmarks.
Furthermore, in accordance with some embodiments of the present invention, the bias errors associated with the measured bearings comprise errors associated with a mechanical deficiency of the sensor.
Furthermore, in accordance with some embodiments of the present invention, the method used for correcting errors associated with a mechanical deficiency of the sensor comprises calculating a corrected angle of bearing to each of the landmarks, β<sub>corr i</sub>.
Furthermore, in accordance with some embodiments of the present invention, calculating a corrected angle of bearing to each of the landmarks, β<sub>corr i </sub>comprising finding a correction function denoted ƒ that is defined to be continuous such that the corrected bearing β<sub>corri </sub>for each of the landmarks is:
β<sub>corr</sub><sub><sub2>i</sub2></sub><sub>i</sub>=/β<sub>i</sub>+ƒ(β<sub>i</sub>) where β<sub>i </sub>is the angle of obtained bearing for each of the landmarks.
Furthermore, in accordance with some embodiments of the present invention, the given positions of the landmarks were inaccurately determined and the bias errors associated with the measured bearings comprise errors associated with the inaccurate determination of the positions of the landmarks.
Furthermore, in accordance with some embodiments of the present invention, correcting errors associated with inaccurate determination of the positions of the landmarks comprising:
using the sensor to obtain several bearing sets of at least three landmarks as the robot changes its position;
calculating optimal positions of the landmarks that minimize the sum of square error values over the bearing sets, and
using a similarity transformation for transforming the optimal positions of the landmarks to determine positions of the landmarks that are closest to the given positions of the landmarks.
Furthermore, in accordance with some embodiments of the present invention, the bias errors associated with the measured bearings comprise errors associated with a mechanical deficiency of the sensor, also comprise errors associated with inaccurate determination of the positions of the landmarks.
Furthermore, in accordance with some embodiments of the present invention, the bias errors associated with the measured bearings comprise errors attributed to rotations of the robot.
Furthermore, in accordance with some embodiments of the present invention, the correction function is used to estimate a rotation speed of the robot that minimizes error values attributed to the rotations of the robot.
Furthermore, in accordance with some embodiments of the present invention, the bias errors associated with the measured bearings comprise errors attributed to misidentification of the landmarks.
Furthermore, in accordance with some embodiments of the present invention, determining existence of misidentified landmarks comprising:
checking if the sum of the squared errors between measured bearings and the corrected bearings is above a predetermined threshold;
for a randomly chosen subset of the landmarks determining the location and the orientation of the robot to be where the sum of squared errors between measured bearings and the corrected bearings is minimal;
calculating for the landmarks that were not included in the subset the errors between measured bearings and the corrected bearings;
if for most of the landmarks the errors are below a predetermined value, identifying and disregarding landmarks where the errors were above the predetermined value as the misidentified landmarks; and
if for most of the landmarks the errors are over a predetermined value, randomly choosing a new subset of the landmarks and repeating the steps of determining the location and the orientation of the robot to be where the sum of squared errors between measured bearings and the corrected bearings is minimal;
calculating for the landmarks that were not included in the new subset the errors between measured bearings and the corrected bearings, until the errors for most of the landmarks are below the predetermined value.
BRIEF DESCRIPTION OF THE DRAWINGSThe subject matter regarded as the invention is particularly pointed out and distinctly claimed in the concluding portion of the specification. The invention, however, both as to organization and method of operation, together with objects, features, and advantages thereof, may best be understood by reference to the following detailed description when read with the accompanying drawings in which:
FIG. 1 is a flow chart illustrating stages of a localization method in accordance with embodiments of the present invention
FIG. 2 is a schematic illustration of a laserNav sensor manufactured by Denning Mobile Robotics, Inc.
FIG. 3A illustrates an example of a landmark.
FIG. 3B illustrates an example of a misidentified landmark.
FIG. 4 schematically illustrates distorted results obtained by the localization method.
FIG. 5 schematically illustrates 3 distorted experimental results and the accompanied corrected profiles obtained by the localization method.
FIG. 6 illustrates a table providing landmark coordinate vectors and the sum of the associated error values.
FIG. 7 is a schematic illustration of a distorted angle measurement due to robot rotation.
FIG. 8 is a plot of the error value of a scan vs. the assumed robot speed.
FIG. 9A illustrates distorted localization results obtained during robot rotation.
FIG. 9B illustrates a corrected profile of localization results obtained during robot rotation.
FIG. 10 is a flow chart illustrating stages of a method to be used in order to overcome the problem of index misidentification in accordance with embodiments of the present invention.
FIG. 11 is a flow chart illustrating stages of a method based on “The Complete Approach” for eliminating misidentified landmarks in accordance with embodiments of the present invention.
FIG. 12 is a flow chart illustrating stages of a method for eliminating misidentified landmarks based on the “RANSAC” approach.
FIG. 13 is a table illustrating results of the method used for eliminating misidentified landmarks based on the “RANSAC” approach
FIG. 14 schematically illustrates a robot with a mounted calibration grid used for measures carried out by a camera.
FIG. 15 illustrates the results of an experiment in which the robot positions were measured by the localization method and by a camera.
It will be appreciated that for simplicity and clarity of illustration, elements shown in the figures have not necessarily been drawn to scale. For example, the dimensions of some of the elements may be exaggerated relative to other elements for clarity. Further, where considered appropriate, reference numerals may be repeated among the figures to indicate corresponding or analogous elements.
DETAILED DESCRIPTION OF THE PRESENT INVENTIONIn the following detailed description, numerous specific details are set forth in order to provide a thorough understanding of the invention. However, it will be understood by those skilled in the art that the present invention may be practiced without these specific details. In other instances, wellknown methods, procedures, and components have not been described in detail so as not to obscure the present invention.
Localization is required in order to enable a mobile robot to know its location in the workplace. This is an essential component for path planning to specific locations and for preventing the robot from reaching undesired locations.
Therefore, a localization method that is used for estimating the position and orientation of a mobile robot is disclosed in the present invention.
The localization method, in accordance with embodiments of the present invention, allows estimating the position and orientation of a mobile robot with respect to landmarks whose positions are given. The robot is provided with a sensor for measuring bearings of the landmarks with respect to the robot, where bias errors are present.
The localization method, in accordance with embodiments of the present invention, is a mathematical method which overcomes four sources of failures: (a) Biased bearing measurement errors occurred due to mechanical deficiency of the sensor and its mounting on the robot, (b) Inaccurate manual measurements of the positions of the landmarks fed into the algorithm, (c) Distorted measurements during robot rotations, and (d) Misidentification of landmarks.
FIG. 1 is a flow chart 100 illustrating stages of a localization method in accordance with embodiments of the present invention. The localization method according to embodiments of the present invention comprises: using a sensor to obtain bearings to the landmarks 102, estimating the location and orientation of the robot with respect to the landmarks based on the measured bearings 104, correcting bias errors associated with the measured bearings to obtain corrected bearings by finding a correction function that minimizes the sum of squared errors between measured bearings and the corrected bearings 106, and determining the location and the orientation of the robot to be where the sum of squared errors between measured bearings and the corrected bearings is minimal 108.
FIG. 2 is a schematic illustration of a laserNav sensor 200 (manufactured by Denning Mobile Robotics, Inc.) which is a precision optical positioning sensor that can be used on a moving platform 202 such as automated guided vehicle systems (AGVS) and mobile robots to give exact position and orientation information supporting autonomous navigation, guidance and path following.
It should be noted that similar sensors of other manufacturers may be used as well.
As seen in FIG. 2, LaserNav sensor 200 uses an eye safe scanning laser 204 to produce beam 206. Beam 206 passes through a path directing mirror 208 and is used for detecting landmarks such as landmark 210.
These landmarks can be made from a retroreflective material such as, but not limited to, a material named 2000X manufactured by 3M, U.S.A, so they reflect and return laser beam 206 to a scanner, unlike other objects, that scatter the beam.
Thus, LaserNav sensor 200 senses such landmarks and sends them to a computer.
LaserNav sensor 200 has a full 360 degree field of view except for two 4 degree blind spots caused by support posts such as support post 212.
FIG. 3A illustrates an example of a landmark 300.
Landmark 300 consists of alternate strips of retroreflective material and diffuse reflecting material. Landmark index 301 is coded with 5 data bits, “one” bit 302 and “zero” bit 304 which have a different length.
Indexing landmark 300 gives:
<FORM>1*2<sup>0</sup>+0*2<sup>1</sup>+1*2<sup>2</sup>+1*2<sup>3</sup>+1*2<sup>4</sup>=29</FORM>
It may happen that due to partial occlusion of the landmark or insufficient lighting conditions, a landmark index may include errors.
FIG. 3B illustrates an example of a misidentified landmark 350.
Concealing element 352 causes misidentification of bit 354. The index of the misidentified landmark is:
<FORM>1*2<sup>0</sup>+0*2<sup>1</sup>+1*2<sup>2</sup>+0*2<sup>3</sup>+1*2<sup>4</sup>=21</FORM>
To derive a relation between the position of a landmark and the position and orientation of a robot, the inventors perform a coordinate transformation from the room coordinate system to the robot coordinate system.
The inventors consider a set of n landmarks whose positions are P<sub>i</sub>εR<sup>2</sup>, wherein the robot position P and orientation θ are unknown. Then, the transformation of a single bearing yields the following:
<FORM>Mi=li(cos βi,sin βi)=R(Pi−P)=RPi+T (1)</FORM>
where Mi are the coordinates of the bearing in the robot coordinate system, R is a planar rotation matrix by θ, β<sub>i </sub>is the measured angle of the bearing to the landmark, li is the unknown distance from P to Pi and T=−RP.
This formulation leads to a linear homogeneous equation with four unknowns. When three measurements are available together with an additional structural constraint, the problem can be solved and the position and the orientation of the robot may be computed.
If more than three landmarks are available, the least squares solution is obtained, and the sum of square errors provide a quality measure.
Estimation of the accuracy is done by finding the difference between the measured angles of the bearings and the angles calculated based on the given positions of the landmarks wherein given positions of the landmarks refer to initially measured landmark positions either manually or by any other way.
Thus, if (P<sub>x</sub>, P<sub>y</sub>) is the position of the robot which is obtained by the localization method, and (P<sub>ix</sub>, P<sub>iy</sub>) is the position of landmark i, estimating the angle of the bearing is therefore:
<FORM>β<sub>ci</sub>=arctan 2(Δxi,Δyi) (2)</FORM>
where Δx<sub>i</sub>=Pi<sub>x</sub>−Px, Δy<sub>i</sub>=Pi<sub>y</sub>−Py
The angular error of a single landmark is:
<FORM>Δβi=(βc<sub>i</sub>−θ)−βi (3)</FORM>
wherein βc<sub>i </sub>is the calculated angle of the bearing, βi is the measured angle of the bearing, and θ is the orientation angle of the robot.
The error value associated with the results obtained by the localization method is the average of the sum of square errors of all the landmarks of the scanned set (hereinafter referred to as “subset”).
<maths id="MATHUS00002" num="00002"><math overflow="scroll"><mtable><mtr><mtd><mrow><mi>S</mi><mo>=</mo><mrow><munderover><mo>∑</mo><mrow><mi>i</mi><mo>=</mo><mn>1</mn></mrow><mi>n</mi></munderover><mo></mo><mfrac><mrow><mi>Δ</mi><mo></mo><mstyle><mspace width="0.3em" height="0.3ex"/></mstyle><mo></mo><msubsup><mi>β</mi><mi>i</mi><mn>2</mn></msubsup></mrow><mi>n</mi></mfrac></mrow></mrow></mtd><mtd><mrow><mo>(</mo><mn>4</mn><mo>)</mo></mrow></mtd></mtr></mtable></math></maths>
When S is greater than some threshold, the measurement is considered wrong.
The mathematical method described above for estimating, the error value associated with measures obtained by the localization method is a basic method used for handling various problems such as those mentioned earlier i.e., biased bearing measurement errors due to mechanical deficiency of the LaserNav sensor, inaccurate given positions of the landmarks, distorted measurements during robot rotations and misidentification of landmarks.
FIG. 4 schematically illustrates distorted measures 400 of the robot position.
The position (P<sub>x</sub>,P<sub>y</sub>) of the robot which was calculated by the localization method was supposed to be close to constant, but due to the mechanical deficiency of the LaserNav sensor and the robot, the results were grossly distorted and dispersed over a diameter of about 7 cm 402 as can be seen in FIG. 4.
To overcome biased bearing measurement errors attributed to mechanical deficiency of the sensor, the inventors aim at finding a bias correction function ƒ for each landmark bearing βi such that:
<FORM>β<sub>corr</sub><sub><sub2>i</sub2></sub>=β<sub>i</sub>+ƒ(β<sub>i</sub>)</FORM>
where is the corrected angle.
The average value of ƒ must be zero since otherwise the correcting function contributes a constant rotation component to the robot.
In addition, due to the nature of the distortion, ƒ is continuous and periodic, and thus is best represent by its Fourier coefficients.
The inventors make a seconddegree approximation of the function and get
<FORM>ƒ(βi)=A cos(β<sub>i</sub>)+B sin(β<sub>i</sub>)+C cos(2β<sub>i</sub>)+D sin(2β<sub>i</sub>)+K (5)</FORM>
In order to find the coefficients A, B, C, and D, the inventors use a minimization technique that minimizes the sum of error values over the measurement set where K, the average value of the function, is zero. Thus,
<maths id="MATHUS00003" num="00003"><math overflow="scroll"><mtable><mtr><mtd><mrow><mrow><mo>{</mo><mrow><mi>A</mi><mo>,</mo><mi>B</mi><mo>,</mo><mi>C</mi><mo>,</mo><mi>D</mi></mrow><mo>}</mo></mrow><mo>=</mo><mrow><mi>arg</mi><mo></mo><mstyle><mspace width="0.3em" height="0.3ex"/></mstyle><mo></mo><mi>min</mi><mo></mo><mrow><munderover><mo>∑</mo><mrow><mi>j</mi><mo>=</mo><mn>1</mn></mrow><mi>M</mi></munderover><mo></mo><msub><mi>S</mi><mi>j</mi></msub></mrow></mrow></mrow></mtd><mtd><mrow><mo>(</mo><mn>6</mn><mo>)</mo></mrow></mtd></mtr></mtable></math></maths>
where M is the number of the scanned subsets in the set, and Sj is the error value associated with the localization result of subset j.
The estimated angular error is determined as:
<FORM>Δβ<sub>i</sub>=β<sub>ci</sub>−θ−(β<sub>i</sub>+ƒ(β<sub>i</sub>))</FORM>
FIG. 5 schematically illustrates 3 distorted experimental results and the accompanied corrected profiles which were obtained by the localization method.
The distortions in this case are due to mechanical deficiencies at three different (x,y) positions of the robot.
As noted earlier, the inventors use a minimization technique to derive equation (6) for correcting the distorted profiles shown in FIG. 5. They use the same correction parameters (i.e., ABCD=[−0.0080927 0.0004992 0.00098311 −4.1253e005]) and transform twisted circles 502, 504, and 506 into circles 508, 510, and 512 each having an approximate diameter of 1.5 cm while 95% of the points lie within a 1 cm diameter.
Another problem handled by the localization method in accordance with embodiments of the present invention is associated with inaccurate given positions of landmarks.
It should be noted that the given positions of the landmarks may be manually measured or otherwise.
Inaccurate given positions of landmarks can distort the localization results. Furthermore, the degree of distortion may increase if the inaccuracies are associated with measurements that depend on one another (i.e., if the position of a landmark is measured relative to another landmark in the group).
It should be noted that errors attributed to inaccurate given positions of the landmarks may be sometimes of the same order of magnitude as the error associated with landmark index misidentification.
As will be discussed later in the text, errors associated with landmark index misidentification cause the elimination of those landmarks from a measurement (i.e., from a subset) and an undesired removal of an entire subset.
Thus, errors attributed to inaccurate given positions which are of the same order of magnitude as errors associated with landmark index misidentification may cause the elimination of landmarks from the measurement and an undesired removal of an entire subset.
To avoid the abovementioned scenario, it is required to reduce the amplitude of errors associated with inaccurate given positions of the landmarks and force them to be normally distributed (i.e., to have a Gaussian distribution).
This is done by a twostep algorithm. In the first step, the optimal positions of the landmarks are calculated minimizing the sum of the error values (eq. 4) over the scanned subsets. The resulting positions, however, are invariant to planar similarity transformations. Therefore, the second step involves with finding a similarity transformation for transforming the resulting positions to yield positions which are closest to the initial given positions of the landmarks.
Thus, the obtained positions coincide as much as possible with the given positions of the bearings.
A detailed description of the twostep algorithm disclosed above is provided in the following example:
The inventors use n=11 landmarks and take M=50 measurements (i.e., scanned subsets).
For each scanned subset the robot position is estimated and its error value is computed.
When the landmark positions change, so do the computed robot positions and the error values of the scanned subset.
The inventors aim at finding the landmark positions vector denoted as V<sub>1 </sub>that minimizes the sum of error values over the set of M scanned subsets, and thus, use an optimization program with starting point V<sub>0</sub>.
The inventors assume that the given positions of the landmarks are inaccurate but represent the landmark positions in the room coordinate system.
They also assume that the optimization program distort the (original) given position of the landmarks.
Therefore, they aim at finding a similarity transformation consisting of a rotation matrix R, a translation vector t and a scale parameter s. Applying the similarity transformation to V<sub>1 </sub>yields V<sub>2 </sub>whose distance from the given landmark positions vector V<sub>0 </sub>is minimal.
To solve this problem, the inventors formulate a new minimization problem, where the target function is:
<FORM>{s,γ,t}=arg min∥V<sub>2</sub>−V<sub>1</sub>∥ (7)</FORM>
Subject to:
<FORM>V<sub>2</sub><sub><sub2>i</sub2></sub>=s·R·V<sub>1</sub><sub><sub2>i</sub2></sub>+t </FORM>
where R is a planar rotation matrix by γ and V<sub>2i </sub>are the coordinates of landmark i in the vector V<sub>2</sub>.
The search for minimum starts from s=1, γ=0, t=(0,0) since for those values, V<sub>2i</sub>=V<sub>1i</sub>.
FIG. 6 illustrates a table 600 providing landmark coordinate vectors and the sum of the associated error values.
As seen, in the first two rows of table 600, the V0V1 values are very large and similar. This is due to an inaccurate given position of a first landmark based on which the position of a second landmark was measured.
As seen, the sum of squared errors for V2 is the same as for V1, and the total distance to V0 was reduced from 30.3 cm to 21 cm as a result of the minimization process.
Another problem that the inventors handle by using the disclosed localization method in accordance with embodiments of the present invention is associated with distortions added to the LaserNav sensor measurements during robot rotations.
According to their experimental results, the inventors found that the distortion added to the LaserNav sensor measurements while the robot rotates depends the rotation speed of the robot. Therefore, they aim at finding an optimized rotation speed of the robot that minimizes the error value associated with the motion of the robot for all measurements. More specifically, the inventors aim at finding a rotation speed which minimizes the vectorial angle added to the angular measurement of a landmark bearing with respect to the robot position.
FIG. 7 is a schematic illustration of a distorted angle measurement due to robot rotation.
The inventors propose a model according which a mirror (mirror 208 shown in FIG. 2) rotates 702 clockwise at a speed ν<sub>mirr</sub>=10 RPS, and the robot rotates 704 counterclockwise at a speed of ν<sub>r</sub>.
Suppose that a landmark bearing βi 706 is observed, the scan starts at point 708 at time t<sub>0 </sub>and the landmark is detected at time t<sub>i</sub>.
Then Δt<sub>i</sub>=ti−t<sub>0</sub>=βi/ν<sub>mirr</sub>. At this time the mirror has rotated β<sub>mirr</sub><sub><sub2>i</sub2></sub>=ν<sub>mirr</sub>·Δt<sub>i </sub>and the robot rotated β<sub>robi</sub>=ν<sub>rob</sub>·Δt<sub>i </sub>causing a distorted bearing 710 to be observed (bearing 712 is the true bearing which is supposed to be observed when the distortion attributed to the rotation of the robot is overcomed).
Both angles i.e., the angle due to the rotation of the robot, βrobi, and the angle due to the rotation of the mirror, βmirri donate to the measured bearing:
<FORM>βi=βrobi+βmirri. </FORM>
The inventors are interested in isolating the mirror rotation, and thus subtract β<sub>rob</sub>,
<FORM>βmirri=βi−βrobi. </FORM>
To find a velocity of the robot that minimizes the error value of the result for all the observed landmarks, the inventors define the error of a single landmark as:
<FORM>Δβ<sub>i</sub>=(β<sub>c</sub><sub><sub2>i</sub2></sub>−θ)−β<sub>mirr</sub><sub><sub2>i </sub2></sub></FORM>
and solve the following minimization problem:
<maths id="MATHUS00004" num="00004"><math overflow="scroll"><mtable><mtr><mtd><mrow><msub><mi>v</mi><mi>rob</mi></msub><mo>=</mo><mrow><mi>arg</mi><mo></mo><mstyle><mspace width="0.3em" height="0.3ex"/></mstyle><mo></mo><mi>min</mi><mo></mo><mrow><munderover><mo>∑</mo><mrow><mi>i</mi><mo>=</mo><mn>1</mn></mrow><mi>n</mi></munderover><mo></mo><mfrac><mrow><mi>Δ</mi><mo></mo><mstyle><mspace width="0.3em" height="0.3ex"/></mstyle><mo></mo><msubsup><mi>β</mi><mi>i</mi><mn>2</mn></msubsup></mrow><mi>n</mi></mfrac></mrow></mrow></mrow></mtd><mtd><mrow><mo>(</mo><mn>8</mn><mo>)</mo></mrow></mtd></mtr></mtable></math></maths>
Subject to
<maths id="MATHUS00005" num="00005"><math overflow="scroll"><mrow><mrow><mi>Δ</mi><mo></mo><mstyle><mspace width="0.3em" height="0.3ex"/></mstyle><mo></mo><msub><mi>β</mi><mi>i</mi></msub></mrow><mo>=</mo><mrow><mrow><mo>(</mo><mrow><msub><mi>β</mi><mi>ci</mi></msub><mo></mo><mi>θ</mi></mrow><mo>)</mo></mrow><mo></mo><mrow><msub><mi>β</mi><mi>i</mi></msub><mo></mo><mrow><mo>(</mo><mrow><mn>1</mn><mo></mo><mfrac><msub><mi>v</mi><mi>rob</mi></msub><msub><mi>v</mi><mi>mirr</mi></msub></mfrac></mrow><mo>)</mo></mrow></mrow></mrow></mrow></math></maths>
FIG. 8 is a plot 800 of the error value of a scanned subset vs. the assumed robot angular velocity.
The curve produced by the error function vs. the approximated angular velocity of the robot possesses a shape close to the shape of a parabola wherein minimum point 802 corresponds to the correct speed of the robot, i.e., to the speed which minimizes the error associated with the rotation of the robot.
FIGS. 9A&B illustrate the localization results during robot rotation.
FIG. 9A illustrates distorted localization results obtained during robot rotation.
FIG. 9B illustrates a corrected profile of localization results obtained during robot rotation.
In this experiment the robot was rotating at speeds up to 45°/second around its axis at a fixed position. 170 scans were taken during the experiment and for each scan the angular velocity was estimated independently.
The corrected results lie within a circle 902 with a diameter of 2 cm, while the raw results are scattered within a circle 904 with a diameter of 30 cm.
The sum of the error values over the set of scans is 2.80E05 for the corrected results as compared to 1.68E02 for the raw results.
These results clearly show that the localization method proposed by the inventors for correcting the localization during robot rotation is essential and highly efficient in minimizing the error associated with angular measurements while the robot rotates.
Another problem handled by using the disclosed localization method in accordance with embodiments of the present invention is associated with sensor misidentification of landmarks.
As noted earlier, partial concealment of landmarks is the main cause of misidentification of landmarks. When an object conceals a part of a reflective bit on the landmark, the sensor can read the value of such bit as “0” instead of “1”.
An example of such case is shown in FIG. 3B. The meaning of such an error for the localization system is that it may detect some landmarks at angles which are completely incorrect.
FIG. 10 is a flow chart 1000 illustrating stages of a method to be used in order to overcome the problem of index misidentification. The method according to embodiments of the present invention comprises: determining the existence of misidentified landmarks if the sum of the squared errors between measured bearings and the calculated bearings is above a predetermined threshold 1002, for a randomly chosen subset of landmarks, determining the position and the orientation of the robot to be where the sum of squared errors between measured bearings and the calculated bearings is minimal 1004, calculating for the landmarks that were not included in the subset the errors between measured bearings and the calculated bearings 1006, if for most of the landmarks the errors are below a predetermined value, identifying and disregarding landmarks where the errors were above the predetermined value as the misidentified landmarks 1008, and if for most of the landmarks the errors are over a predetermined value, randomly choosing a new subset of the landmarks and repeating the steps of determining the position and the orientation of the robot to be where the sum of squared errors between measured bearings and the calculated bearings is minimal 1010, calculating for the landmarks that were not included in the new subset the errors between measured bearings and the calculated bearings, until the errors for most of the landmarks are below the predetermined value 1012.
The inventors disclose two methods that can be used for eliminating the misidentified landmarks. These methods are illustrated in FIGS. 11 and 12.
FIG. 11 is a flow chart 1100 illustrating stages of a method based on “The Complete Approach” for eliminating misidentified landmarks in accordance with embodiments of the present invention. A method according to embodiments of the present invention comprises: taking all the possible subsets of (n−1) landmarks from the set I 1102, calculating the position and estimating the speed (if required), and the error for each subset 1104, checking whether any of the subsets have an acceptable error value 1106, if any of the subsets has an acceptable error value, accepting it as the measurement result and completing the process 1108, if no set has an acceptable error value, decreasing n by 1 1110, if n>5 (n>4 if the robot is not moving) 1112, going back 1114 to 1102, else, the measurement fails 1116.
Another method for eliminating misidentified landmarks is based on the “RANSAC” approach. In this method, the inventors treat the problem as a set of measurements that contain a majority of true measurements and some wrong measurements and formulate an algorithm based on a technique designed to solve such a scenario that was developed by M. A. Fischler and R. C. Bolles “Random Sample Consensus: A Paradigm for Model Fitting with Applications to Image Analysis and Automated Cartography” in Communications of the ACM Vol. 24, number 6, pages 381395, June 1981.
FIG. 12 is a flow chart 1200 illustrating stages of a method for eliminating misidentified landmarks based on the “RANSAC” approach in accordance with embodiments of the present invention. A method according to embodiments of the present invention comprises: Taking randomly 3 landmarks (5 landmarks if the robot moves) out of the set I of n landmarks and denoting this set A 1202, calculating the localization result for the set A, if the robot moves, estimating also the angular velocity 1204, calculating the error value 1206, defining S=A 1208, for each landmark bεI\A, computing the angle β<sub>ci </sub>according to equation (2) 1210, and computing the angular error according to equation (3) 1212, if the angular error of the landmark is less than some threshold value d, considering that the landmark b agrees with the localization result of the set A 1214, setting S=S∪{b} 1216, if S>I\A/2+A, accepting the result of the set S and finishing 1218, repeating steps 12021218 K times 1220, if did not accept any result, the scan fails 1222.
The value of K is chosen in order to ensure with high probability that a set of correct landmarks is found.
According to the inventors, the Complete Approach is advantageous since it ensures finding the result, (if exists). The RANSAC Approach is advantageous for computing the position only up to 2K times and runs at O(n) complexity.
FIG. 13 is a table 1300 illustrating results of a typical run of the RANSAC method. Table 1300 contains a set of landmarks wherein one landmark is misidentified.
The top row 1303 of table 1300 shows a result based on all the observed landmarks that invoked the RANSAC method because of its high error value. In the other rows “(+)” denotes landmarks belonging to randomly selected subsets used for localizing the robot, “+” denotes landmarks whose bearings agree with the computed position and orientation of the robot (i.e., the localization result) and “−” denotes landmarks whose bearings do not agree with the computed position and orientation of the robot.
All landmarks excluding landmark 1304 agree with the localization results. Therefore, the inventors conclude that landmark 1304 is a misidentified landmark. As seen in table 1300, subset 1301 of test 1302 does not include landmark 1304. Therefore the localization result obtained by using subset 1301 is quite accurate and all the landmarks except landmark 1304 agree with the obtained localization result. Thus, it is concluded that landmark 1304 is a misidentified landmark.
To test and verify the validity of the results obtained by using the localization method in accordance with embodiments of the present invention, the inventors compare the position and orientation obtained by the localization system to position and orientation values obtained by a proven method that measures the position and orientation of the robot accurately with a camera.
FIG. 14 schematically illustrates a robot 1400 with a mounted calibration grid 1402 and relevant coordinate systems i.e., robot coordinate system 1404, grid coordinate system 1406, and room coordinate system 1408.
As seen, the grid is mounted on the robot, and corresponding measurements of the grid position and orientation are taken for some of the measurements of the localization system.
FIG. 15 illustrates the results 1500 of an experiment in which 15 stationary positions of the robot were measured by the localization method and by a camera.
Seen in FIG. 15, black thick lines 1502 and asterisks 1504 represent measurements obtained by a camera, thin lines 1506 and dots 1508 represent the results obtained by the localization method.
The maximal distance between the results obtained by the localization method and the results obtained by the camera was found to be 1.32 cm, and the maximal angular difference was found to be 0.12°.
Seen in FIG. 5, the results obtained by the localization method during motion of the robot among the stationary points are smooth and frequent.
While certain features of the invention have been illustrated and described herein, many modifications, substitutions, changes, and equivalents will now occur to those of ordinary skill in the art. It is, therefore, to be understood that the appended claims are intended to cover all such modifications and changes as fall within the true spirit of the invention.