Visual Robot Pose Estimation

Patent metadata

Field	Value
Publication number	US-2025068165-A1
Application number	US-202318454126-A
Country	US
Kind code	A1
Filing date	Aug 23, 2023
Priority date	Aug 23, 2023
Publication date	Feb 27, 2025
Grant date	—

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 on the robot an image of the object in the environment, the image formed on an image plane of the camera including two observed points (op 1 , op 2 ) corresponding to the two co-planar points (P 1 , P 2 ) on the object; the image further including 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 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 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 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 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 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 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 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 includes performing a further 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 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 following formula:

E
lines

=



n
ae

→

-

n
ae
′

→



+



n
bf

→

-

n
bf
′

→



Wherein n ae and n bf 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 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
′

→



+



n
bf

→

-

n
bf
′

→

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