Method and device for estimating pose of electric vehicle charging socket and autonomous charging robot employing the same

1. A method of controlling an autonomous electric vehicle charging robot, the method comprising:

acquiring, by an image sensor, an RGB image and a depth map of a charging socket of an electric vehicle;

detecting, by a processor, keypoints of the charging socket based on the RGB image;

deriving, by the processor, a first estimated pose of the charging socket based on the depth map;

deriving, by the processor, a second estimated pose of the charging socket based on the keypoints of the charging socket and the first estimated pose; and

driving a charging connector of the autonomous electric vehicle charging robot according to the second estimated posture of the charging socket so that the charging connector is moved to be aligned with the charging socket and is inserted into the charging socket,

wherein the deriving the first estimated pose of the charging socket comprises:

correcting the depth map of the charging socket by interpolating the depth map;

deriving an equation of a plane of the charging socket by sampling a plurality of internal points inside a corrected depth map;

projecting the keypoints of the charging socket onto the plane represented by the equation to acquire projection coordinates;

acquiring reference coordinates (q i ) corresponding to the projection coordinates (p i ) from a CAD model of the charging socket; and

acquiring an initial transformation matrix describing the first estimated pose and minimizing differences between the projection coordinates (p i ) and the reference coordinates (q i ), wherein the deriving the second estimated pose of the charging socket comprises:

deriving an optimal transformation matrix describing the second estimated pose, comprising an optimal rotation matrix and an optimal translation vector, and minimizing differences between coordinates of the keypoints (y i ) and the reference coordinates (q i ) corresponding to the keypoints.

2. The method of claim 1 , wherein the detecting the keypoints of the charging socket based on the RGB image comprises:

detecting a bounding box surrounding the charging socket, calculating a confidence score for coordinates of the bounding box, and determining a class indicating a type of the charging socket by a learnable charging socket detection neural network; and

detecting the keypoints inside a charging socket region in the bounding box by a learnable keypoint detection neural network.

3. The method of claim 2 , wherein the charging socket detection neural network comprises a MobileNetV3 as a backbone and a MnasFPN as a detection head.

4. The method of claim 2 , wherein the detecting the bounding box surrounding the charging socket comprises:

removing at least one bounding box overlapping another bounding box among a plurality of bounding boxes detected by the charging socket detection neural network by a non-maximum suppression.

5. The method of claim 2 , wherein the keypoint detection neural network detects the keypoints based on a single RGB image and the bounding box by using only a convolutional pose machine during an actual operation after a learning.

6. The method of claim 1 , wherein the

initial transformation matrix is determined by a following equation:

[

R

,

t

]

=

arg

⁢

min

R

,

t

⁢

∑

i

=

1

n

w

i

⁢



Rq

i

+

t

-

p

i



2

where ‘p i ’ denotes the projection coordinates, ‘q i ’ denotes the reference coordinates corresponding to the projection coordinates, ‘R’ and ‘t’ denote a rotation matrix and a translation vector, respectively, of the initial transformation matrix, and ‘w i ’ denotes weights for respective pairs of the projection coordinates and the reference coordinates.

7. The method of claim 6 , wherein the

optimal transformation matrix is determined by a following equation:

[

R

*

,

t

*

]

=

arg

⁢

min

R

*

,

t

*

⁢

∑

i

=

1

n



K

[

R

*

❘

t

*

]

⁢

q

i

-

y

i



2

where ‘y i ’ denotes coordinates of the keypoints in the RGB image corresponding to the reference coordinates, ‘R*’ and ‘t*’ denote the optimal rotation matrix and the optimal translation vector, respectively, of the optimal transformation matrix, and ‘K’ denotes a camera intrinsic parameter.

8. A device for controlling an autonomous electric vehicle charging robot, comprising:

a processor; and

a memory storing program instructions executable by the processor,

wherein the program instructions, when executed by the processor, cause the processor to:

acquire an RGB image and a depth map of a charging socket of an electric vehicle;

detect keypoints of the charging socket based on the RGB image;

derive a first estimated pose of the charging socket based on the depth map;

derive a second estimated pose of the charging socket based on the keypoints of the charging socket and the first estimated pose; and

drive a charging connector of the autonomous electric vehicle charging robot according to the second estimated posture of the charging socket so that the charging connector is moved to be aligned with the charging socket and is inserted into the charging socket,

wherein the program instructions causing the processor to derive the first estimated pose of the charging socket comprises program instructions causing the processor to:

correct the depth map of the charging socket by interpolating the depth map;

derive an equation of a plane of the charging socket by sampling a plurality of internal points inside a corrected depth map;

project the keypoints of the charging socket onto the plane represented by the equation to acquire projection coordinates;

acquire reference coordinates (q i ) corresponding to the projection coordinates (p i ) from a CAD model of the charging socket; and

acquire an initial transformation matrix describing the first estimated pose and minimizing differences between the projection coordinates (p i ) and the reference coordinates (q i ),

wherein the program instructions causing the processor to derive the second estimated pose of the charging socket comprises program instructions causing the processor to:

derive an optimal transformation matrix describing the second estimated pose, comprising an optimal rotation matrix and an optimal translation vector, and minimizing differences between coordinates of the keypoints (y i ) and the reference coordinates (q i ) corresponding to the keypoints.

9. The device of claim 8 , wherein the program instructions causing the processor to detect the keypoints of the charging socket based on the RGB image comprises program instructions causing the processor to:

detect a bounding box surrounding the charging socket, calculate a confidence score for coordinates of the bounding box, and determine a class indicating a type of the charging socket by a learnable charging socket detection neural network; and

detect the keypoints inside a charging socket region in the bounding box by a learnable keypoint detection neural network.

10. The device of claim 9 , wherein the charging socket detection neural network comprises a MobileNetV3 as a backbone and a MnasFPN as a detection head.

11. The device of claim 9 , wherein the program instructions causing the processor to detect the bounding box surrounding the charging socket comprises program instructions causing the processor to:

remove at least one bounding box overlapping another bounding box among a plurality of bounding boxes detected by the charging socket detection neural network by a non-maximum suppression.

12. The device of claim 9 , wherein the keypoint detection neural network detects the keypoints based on a single RGB image and the bounding box by using only a convolutional pose machine during an actual operation after a learning.

13. The device of claim 8 , wherein the

initial transformation matrix is determined by a following equation:

[

R

,

t

]

=

arg

⁢

min

R

,

t

⁢

∑

i

=

1

n

w

i

⁢



Rq

i

+

t

-

p

i



2

where ‘p i ’ denotes the projection coordinates, ‘q i ’ denotes the reference coordinates corresponding to the projection coordinates, ‘R’ and ‘t’ denote a rotation matrix and a translation vector, respectively, of the initial transformation matrix, and ‘w i ’ denotes weights for respective pairs of the projection coordinates and the reference coordinates.

14. The device of claim 13 , wherein the

optimal transformation is determined by a following equation:

[

R

*

,

t

*

]

=

arg

⁢

min

R

*

,

t

*

⁢

∑

i

=

1

n



K

[

R

*

❘

t

*

]

⁢

q

i

-

y

i



2

where ‘y i ’ denotes coordinates of the keypoints in the RGB image corresponding to the reference coordinates, ‘R*’ and ‘t*’ denote an optimal rotation matrix and an optimal translation vector, respectively, of the optimal transformation matrix, and ‘K’ denotes a camera intrinsic parameter.

15. An autonomous electric vehicle charging robot, comprising:

a manipulator having six degrees of freedom and configured to be controllable to insert a charging connector into a charging socket of an electric vehicle;

an image acquirer configured to acquire an RGB image and a depth map of the charging socket; and

a robot controller configured to estimate a pose of the charging socket, determine a driving angle of each joint of the manipulator based on an estimated pose of the charging socket, determine whether the charging connector and the charging socket are aligned with each other, and control the manipulator to insert the charging connector into the charging socket,

wherein the robot controller comprises:

a processor and a memory storing program instructions executable by the processor, wherein the program instructions, when executed by the processor, cause the processor to:

detect keypoints of the charging socket based on the RGB image;

correct the depth map of the charging socket by interpolating the depth map;

derive an equation of a plane of the charging socket by sampling a plurality of internal points inside a corrected depth map;

project the keypoints of the charging socket onto the plane represented by the equation to acquire projected coordinates;

acquire reference coordinates (gi) corresponding to the projection coordinates (p i ) from a CAD model of the charging socket;

acquire an initial transformation matrix describing a first estimated pose and minimizing differences between the projection coordinates (p i ) and the reference coordinates (q i ); and

derive a second estimated pose of the charging socket as the pose of the charging socket based on the keypoints of the charging socket and the first estimated pose by determining an optimal transformation matrix which describes the second estimated pose, comprises an optimal rotation matrix and an optimal translation vector, and minimizes differences between coordinates of the keypoints (y i ) and the reference coordinates (q i ) corresponding to the keypoints.

16. The autonomous electric vehicle charging robot of claim 15 , wherein the program instructions causing the processor to detect the keypoints of the charging socket based on the RGB image comprises program instructions causing the processor to:

detect a bounding box surrounding the charging socket, calculate a confidence score for coordinates of the bounding box, and determine a class indicating a type of the charging socket by a learnable charging socket detection neural network; and

detect the keypoints inside a charging socket region in the bounding box by a learnable keypoint detection neural network.

17. The autonomous electric vehicle charging robot of claim 16 , wherein the charging socket detection neural network comprises a MobileNetV3 as a backbone and a MnasFPN as a detection head.

18. The autonomous electric vehicle charging robot of claim 16 , wherein the program instructions causing the processor to detect the bounding box surrounding the charging socket comprises program instructions causing the processor to:

remove at least one bounding box overlapping another bounding box among a plurality of bounding boxes detected by the charging socket detection neural network by a non-maximum suppression.

19. The autonomous electric vehicle charging robot of claim 15 , wherein the

initial transformation matrix is determined by a following equation:

[

R

,

t

]

=

arg

⁢

min

R

,

t

⁢

∑

i

=

1

n

w

i

⁢



Rq

i

+

t

-

p

i



2

where ‘p i ’ denotes the projection coordinates, ‘q i ’ denotes the reference coordinates corresponding to the projection coordinates, ‘R’ and ‘t’ denote a rotation matrix and a translation vector, respectively, of the initial transformation matrix, and ‘w i ’ denotes weights for respective pairs of the projection coordinates and the reference coordinates.