Visual robot pose estimation

Patent metadata

Field	Value
Publication number	US-12372967-B2
Application number	US-202318454126-A
Country	US
Kind code	B2
Filing date	Aug 23, 2023
Priority date	Aug 23, 2023
Publication date	Jul 29, 2025
Grant date	Jul 29, 2025

Abstract

A method for estimating a pose of a robot using two co-planar points on an object plane in the environment and two co-planar lines on the object plane, includes capturing with a camera on the robot an image of the object in the environment, including two observed points corresponding to the two co-planar points on the object and two observed lines corresponding to the two co-planar lines on the object; projecting onto the image plane the two co-planar points to obtain two projected points and projecting the two co-planar lines to obtain two projected lines; determining a point projection error by comparing the two projected co-planar points to corresponding observed points; determining a line projection error by comparing the two projected co-planar lines to corresponding observed lines; and estimating a current robot pose if the point projection error and the line projection error are below a predetermined error threshold.

First claim

What is claimed is:

1. A method for estimating a pose of a robot traveling in an environment using two co-planar points (P 1 , P 2 ) on an object plane on an object in the environment and two co-planar lines (P 1 -A, P 2 -B) on the object plane on the object, the method comprising
capturing with a camera mounted on the robot images of the environment, including images of the object in the environment;
receiving camera images from the camera by a video decoder, including a camera image of the object;
decoding by the video decoder the received camera images into decoded digital images, including a decoded digital image of the object;
identifying, using a processor and memory, on the decoded digital image of the object two observed points (op 1 , op 2 ) corresponding to the two co-planar points (P 1 , P 2 ) on the object;
identifying, using the processor and memory, on the decoded digital image of the object two observed image lines (op 1 - a , op 2 - b ) corresponding to the two co-planar lines (P 1 -A, P 2 -B) on the object;
projecting, using the processor and memory, onto the image plane of the camera the two co-planar points (P 1 , P 2 ) on the object plane to obtain two projected co-planar image points (pp 1 , pp 2 ) and projecting, using the processor and memory, the two co-planar lines (P 1 -A, P 2 -B) on the object plane to obtain two projected co-planar image lines (pp 1 - a , pp 2 - b );
determining, using the processor and memory, a point projection error by comparing the two projected co-planar image points (pp 1 , pp 2 ) to corresponding observed points (op 1 , op 2 );
determining, using the processor and memory, a line projection error by comparing the two projected co-planar image lines (pp 1 - a , pp 2 - b ) to corresponding observed image lines (op 1 - a , op 2 - b ); and
estimating, using the processor and memory, a current robot pose if the point projection error and the line projection error are below a predetermined error threshold.

2. The method of claim 1 further including combining, using the processor and memory, the point projection error and the line projection error to establish a combined error, E total , and comparing the E total to the predetermined error threshold.

3. The method of claim 2 further including performing, using the processor and memory, a non-linear optimization on the two observed points (op 1 , op 2 ) relative to the two projected points (pp 1 , pp 2 ) and the two observed image lines (op 1 - a , op 2 - b ) and relative to the two projected co-planar image points (pp 1 , pp 2 ) before determining the combined error, E total .

4. The method of claim 3 wherein if the combined error, E total , is not less than the error threshold, the method further includes performing, using the processor and memory, a non-linear optimization on the two observed points (op 1 , op 2 ) relative to the two projected points (pp 1 , pp 2 ) and the two observed image lines (op 1 - a , op 2 - b ) and re-determining the combined error, E total .

5. The method of claim 2 wherein the point projection error is determined, using the processor and memory, according to the following formula:

E
points

=

∑
i



q
i

-

s
i



wherein q i are the projected points on image plane and s i are the observed points on the image plane.

6. The method of claim 5 wherein the line projection error is determined, using the processor and memory, according the following formula:

E
lines

=



n
ae

→

-

n
ae
′

→



+



n
bf

→

-

n
bf
′

→



wherein n ae and n′ ae are the projected lines on image plane and n bf and n′ bf are the observed lines on the image plane.

7. The method of claim 6 wherein the combined point projection error and line projection error is determined, using the processor and memory, according the following formula:

E
total

=

w
points

⁢

E
points

+

w
lines

⁢

E
lines

=

w
points

⁢

∑
i



q
i

-

s
i



+

w
lines

(



n
ae

→

-

n
ae
′

Assignees

• Locus Robotics Corp

Inventors

• Li Changchun
• Williams Stephen V

Classifications

• G06T2207/30252

  Vehicle exterior; Vicinity of vehicle · CPC title

• G06T2207/30244

  Camera pose · CPC title

• G06T7/73 Primary

  using feature-based methods · CPC title

• G05D1/0246

  using a video camera in combination with image processing means · CPC title

• G05D1/0225 Primary

  involving docking at a fixed facility, e.g. base station or loading bay (parking aids B62D15/027) · CPC title