Patent ID: 12254646

DETAILED DESCRIPTION

The present disclosure relates to a system and method of refining a six degrees of freedom pose estimate of a target object based on a single one-dimensional measurement. The system includes a control module in electronic communication with a camera and a range-sensing device. The camera is configured to capture image data of the target object, and the range-sensing device is configured to determine the one-dimensional measurement. The range-sensing device determines an actual distance measured between the range-sensing device and an actual point of intersection W′. Specifically, the actual point of intersection W′ represents where a line-of-sight projected by the range-sensing device intersects with the target object. The system determines the six degrees of freedom pose estimate of the target object based on the image data captured by the camera. The system then determines an estimated point of intersection representing where the line-of-sight intersects with the six degrees of freedom pose estimate of the target object. The system then determines an estimated distance measured between the range-sensing device and the estimated point of intersection. The system calculates an absolute error based on a difference between the actual distance and the estimated distance. In an embodiment, the system also determines a reprojection error introduced by the six degrees of freedom pose estimate of the target object. The system then determines a revised pose estimate of the target object based on the absolute error and, if available, the reprojection error.

The following description is merely exemplary in nature and is not intended to limit the present disclosure, application, or uses.

Referring toFIG.1, a system10for refining a six degrees of freedom pose estimate8(seen inFIG.2) of a target object12is illustrated. In the example as shown inFIG.1, the system10is part of an aerial refueling system14located at a tail section16of a tanker or supply aircraft18. The aerial refueling system14includes a boom assembly20connected to a fuselage22of the supply aircraft18at an articulating joint24. The boom assembly20includes a rigid portion26, a telescoping extension28, and a nozzle30. The nozzle30of the boom assembly20is engaged with a fuel receptacle32of a receiver aircraft34during a refueling operation. In the example as shown, the receiver aircraft34, and in particular the fuel receptacle32, represent the target object12. Accordingly, the system10refines the six degrees of freedom pose estimate8(FIG.2) of the fuel receptacle32and the receiver aircraft34. The system10includes a control module40in electronic communication with a camera42and a range-sensing device44. The camera42is positioned in a location to capture image data of the target object12(i.e., the fuel receptacle32and the receiver aircraft34). The control module40predicts the six-degree of freedom pose estimate8of the target object12based on the image data captured by the camera42.

Referring to bothFIGS.1and2, the range-sensing device44is configured to determine an actual distance d between the range-sensing device44(shown inFIG.2) and the target object12(i.e., the fuel receptacle32). The actual distance d represents a single one-dimensional measurement determined by the range-sensing device44. As explained below, the control module40of the system10determines an absolute error associated with the six degrees of freedom pose estimate8of the target object12based on the actual distance d. In an embodiment, the control module40determines a revised six degrees of freedom pose estimate of the target object12based on the absolute error. As explained also below, in another embodiment the control module40also determines a reprojection error associated with estimating the six degrees of freedom pose estimate8, and then determines the revised six degrees of freedom pose estimate based on both the reprojection error and the absolute error.

In the example as shown inFIG.1, the control module40determines a position and an orientation of the boom assembly20based on the revised six degrees of freedom pose estimate. However, it is to be appreciated thatFIG.1is merely exemplary in nature and the system10is not limited to the aerial refueling system14. Indeed, the system10may be used in a variety of other applications where a six degrees of freedom pose estimate of a rigid object is estimated. As seen inFIG.2, the system10includes an extendable arm38. The extendable arm38is represented by the boom assembly20shown inFIG.1, however, it is to be appreciated that the extendable arm38is not limited to the boom assembly20. For example, in another embodiment, the extendable arm38is a robotic arm that grasps and manipulates objects. In this example, the control module40determines a position and an orientation of the extendable arm38based on the revised six degrees of freedom pose estimate as the extendable arm38grasps and manipulates an object.

The camera42sends a video or image feed to the control module40. In the non-limiting embodiment as shown inFIG.1, the camera42is mounted to an underside46of the fuselage22of the supply aircraft18. However, it is to be appreciated that the position of the camera42is not limited to any specific location on the receiver aircraft34. Instead, the camera42is positioned in any location where a field-of-view50of the camera42captures the target object12. For example, in the embodiment as shown inFIG.1, the camera42may be mounted along any number of locations along the underside46of the fuselage22of the supply aircraft18as long as the field-of-view50of the camera42captures the fuel receptacle32and the receiver aircraft34.

The range-sensing device44is any type of device for determining a distance to a specific target location without the need for physical contact. The range-sensing device44includes, but is not limited to, a laser range finder, an ultrasonic sensor, an infrared distance sensor, a light detection and ranging distance (lidar) sensor, or a sonar sensor. In the non-limiting embodiment as shown inFIG.1, the range-sensing device44is mounted statically to a distal end48of the rigid portion26of the boom assembly20. In the example as seen inFIG.2, the range-sensing device44is also mounted to a distal end52of the extendable arm38. As seen in bothFIGS.1and2, the line-of-sight L of the range-sensing device44is aligned with a longitudinal axis A-A of the extendable arm38(or the boom assembly20). Accordingly, the control module40determines the position and the line-of-sight L of the range-sensing device44based on the movement of the extendable arm38. For example, if the extendable arm38is a robotic arm, then the control module40determines the position and the line-of-sight L based on the robotic arm's joint angles.

It is to be appreciated that the range-sensing device44may be located in a variety of locations other than the rigid portion26of the boom assembly20as seem inFIG.1or on the extendable arm38as seen inFIG.2. In other words, the line-of-sight L of the range-sensing device44may not be aligned with the longitudinal axis A-A of the extendable arm38. Instead, the range-sensing device44is positioned in any location where the line-of-sight L of the range-sensing device44intersects with the target object12. For example, in an alternative embodiment, the range-sensing device44is mounted directly adjacent to the camera42on the underside46of the fuselage22of the supply aircraft18.

Referring toFIGS.1and2, the position, orientation, and intrinsic parameters of the camera42are determined in a preliminary off-line camera calibration procedure or, alternatively, the intrinsic parameters are saved in a memory1034(FIG.5) of the control module40. Some examples of the intrinsic parameters of the camera42include, but are not limited to, resolution and aspect ratio. A three-dimensional representation54of the target object12is shown in phantom line inFIG.2. The three-dimensional representation54is also saved in the memory1034of the control module40as well. The control module40is configured to predict the six degrees of freedom pose estimate8of the target object12(i.e., the fuel receptacle32and the receiver aircraft34) based on the image data captured by the camera42using any number of pose estimation approaches. For example, in one non-limiting embodiment, the control module40determines the six-degree of freedom pose estimate8of the target object12based on a perspective-n-point algorithm.

Referring to bothFIGS.1and3, the perspective-n-point algorithm estimates the six degrees of freedom pose estimate8(FIG.2) of the target object12based on a plurality of two-dimensional keypoints60and a plurality of three-dimensional keypoints62(the two-dimensional keypoints60are shown as circles and the three-dimensional keypoints62are shown as crosses). Specifically, the perspective-n-point algorithm requires three or more three-dimensional keypoints62disposed on the target object12. The three-dimensional keypoints62are detected by the control module40based on the image data captured by the camera42. The control module40detects the three or more three-dimensional keypoints62on the target object12(i.e., the fuel receptacle32) in each image frame of the image feed received from the camera42. The control module40then predicts a corresponding two-dimensional keypoint60for each of the plurality of three-dimensional keypoints62using a deep neural network. The control module40then aligns the plurality of three-dimensional keypoints62with the corresponding two-dimensional keypoints60, and then predicts the six degrees of freedom pose estimate based on the three-dimensional keypoints62.

It is to be appreciated that while a perspective-n-point algorithm is described, other pose estimation processes may also be used to determine the six degrees of freedom pose estimate. For example, in an alternative approach, the six degrees of freedom pose estimate is determined based on two or more point-tangent correspondences between the three-dimensional keypoints62and the two-dimensional keypoints60. In another embodiment, the six degrees of freedom pose estimate is determined by deep neural network that determines the six degrees of freedom pose estimate directly based on the image data captured by the camera42.

Referring back toFIGS.1and2, once the control module40determines the six degrees of freedom pose estimate8, the control module40then aligns the longitudinal axis A-A of the extendable arm38in a direction towards the target object12(i.e., the receiver aircraft34). It is to be appreciated that the initial six degrees of freedom pose estimate8as described above may be a locally coarse estimate, and the longitudinal axis A-A of the extendable arm38(and therefore the line-of sight L of the range-sensing device44) only needs to generally intersect with the target object12(i.e., the receiver aircraft34). In the example as shown inFIGS.1and2, since the nozzle30of the boom assembly20engages with the fuel receptacle32of the receiver aircraft34during a refueling operation, the range-sensing device44projects the line-of-sight L towards the fuel receptacle32.

The range-sensing device44is configured to determine the actual distance d. Referring specifically toFIG.2, the actual distance d is measured between the range-sensing device44and an actual point of intersection W′. The line-of-sight L projected by the range-sensing device44intersects with the target object12(i.e., the fuel receptacle32) at the actual point of intersection W′. Thus, the actual distance d represents a one-dimensional depth measurement between the range-sensing device44and the target object12. It is to be appreciated that prior to the pose refinement process using the range-sensing device44, depth estimates, such as estimates of the distance d, are associated with the greatest amount of error when compared to length and height measurements. This is because the initial six degrees of freedom pose estimate is based on a perspective of the camera42, which lacks depth cues. Furthermore, it is also to be appreciated that the position of the actual point of intersection W′ upon a surface70of the receiver aircraft34need not be known. Finally, it is also to be appreciated that the actual point of intersection W′ may lie anywhere upon the surface70of the receiver aircraft34.

Referring specifically toFIG.2, the control module40then determines an estimated point of intersection W. The estimated point of intersection W represents where the line-of-sight L intersects with the six degrees of freedom pose estimate8of the target object12. As seen inFIG.2, the estimated point of intersection W is offset from the actual point of intersection W′ because of the coarseness of the initial six degrees of freedom pose estimate8. The control module40then determines an estimated distance D measured between the range-sensing device44and the estimated point of intersection W. The control module40then calculates the absolute error associated with the six degrees of freedom pose estimate8of the target object12based on a difference between the actual distance d and the estimated distance D. Specifically, the absolute error is expressed in Equation 1 as:
∥W−(O+dL)∥2Equation 1
where O represents a base of the extendable arm38, which is shown inFIG.2, and W′=(O+dL). In other words, Equation 1 may be expressed as ∥W−W′∥2.

In addition to the absolute error, in one embodiment the control module40also determines the reprojection error introduced by the six degrees of freedom pose estimate8. Specifically, the reprojection error represents a difference between a plurality of two-dimensional pixel positions and the plurality of two-dimensional keypoints60shown inFIG.3. The plurality of two-dimensional pixel positions are determined by projecting the three-dimensional keypoints62(FIG.2) into two-dimensional space. It is to be appreciated that the three-dimensional keypoints62shown inFIG.3are represented in camera space. The camera space refers to a three-dimensional coordinate system having an origin represented by a center C of the camera42(FIG.1), where a user defines the three axes (i.e., x, y, and z). Thus, the three-dimensional keypoints62indicate how the target object12appears with respect to the perspective view of the camera42. For example, if the target object12is located 20 meters straight in front of the camera42, then a z-coordinate (which is assumed to be aligned with a line-of-sight of the camera42) of the resulting three-dimensional keypoint62would be 20 meters. It is also to be appreciated that when the three-dimensional keypoints62are projected into the two-dimensional space to represent the two-dimensional pixel locations, the three-dimensional keypoints62are flattened along a depth dimension. However, the range-sensing device44is substantially aligned with the depth dimension, and therefore adds information that is otherwise missing from the two-dimensional pixel locations.

The reprojection error of the perspective-n-point algorithm is expressed in Equation 2 as:
∥P(V[RX+t])−y′∥2Equation 2
where P represents a camera projection function of the camera42, V represents a coordinate transform matrix, R represents a rotation matrix representing the three orientation components (pitch, roll, and yaw) of the six degrees of freedom parameters, X represents a matrix containing the plurality of three-dimensional keypoints62, t represents a vector representing the positional components (x, y, and z), of the six degrees of freedom parameters, and y′ represents the two-dimensional keypoints60(shown inFIG.3). The camera projection function of the camera42converts the three-dimensional keypoints62, which are represented by the camera space, into the two-dimensional dimensional space. The coordinate transform matrix V converts the three-dimensional keypoints62represented in model space into the camera space. The model space represents a three-dimensional coordinate system having an origin74(seen inFIG.2) that is located at a center of the three-dimensional representation54. The vector t contains the positional components of the six degrees of freedom parameters and defines the translation between origin74of the model space and the center C of the camera space. Similarly, the rotation matrix R contains the orientation components of the six degrees of freedom parameters and defines the rotation between the axes defined in model space and the axes defined in camera space.

In one embodiment, the control module40determines the revised six degrees of freedom pose estimate based on just the absolute error. In this embodiment, the control module40determines a minimum value of the absolute error, and then calculates the revised six degrees of freedom pose estimate produces or results in the minimum value of the absolute error. In other words, control module40determines a value for the refined six degrees of freedom pose estimate associated with the least amount of absolute error. The minimum value of the absolute error is expressed in Equation 3 as:

minθ⁢W-(O+dL)2Equation⁢⁢3
where θ represents the six degrees of freedom pose estimate of the target object12, i.e., θ=[x, y, z, pitch, roll, yaw].

In another embodiment, the control module40determines the revised six degrees of freedom pose estimate8based on both the absolute error and the reprojection error. In an embodiment, the control module40determines the revised six degrees of freedom pose estimate by first determining a minimum value of a weighted sum, where the weighted sum combines the absolute error and the reprojection error together. The weighted sum is expressed in Equation 4 as:

minθ⁢P⁡(V⁡[R⁢X+t])-y′2+λ2⁢W-(O+dL)2Equation⁢⁢4
where λ represents a use-defined scale factor. Changing a value of the scale factor λ results in a specific implementation to account for the relative accuracies of the range-sensing device44and the six degree of freedom pose estimate8. The minimum value of the weighted sum is determined based on a non-linear least square algorithm. There are several types of non-linear least square algorithms available that may be used to determine the minimum value of the weighted sum. Some examples of non-linear least square algorithms include, but are not limited to, Gauss-Newton methods, a Levenberg-Marquardt algorithm, a gradient method such as a conjugate-gradient method, and direct search methods such as a Nelder-Mead simplex search.

FIGS.4A-4Bis an exemplary process flow diagram illustrating a method200for refining the six degrees of freedom pose estimate8(FIG.2) of the target object12. Referring generally toFIGS.1-4A, the method200begins at block202. In block202, the camera42captures the image data of the target object12. The method200may then proceed to block204.

In block204, the range-sensing device44determines the actual distance d. As mentioned above, the actual distance d is measured between the range-sensing device44and the actual point of intersection W′ (seen inFIG.2), where the range-sensing device44projects the line-of-sight L that intersects with the target object12at the actual point of intersection W′. The method200may then proceed to block206.

In block206, the control module40predicts, based on the image data of the target object12, the six degrees of freedom pose estimate8of the target object12. As explained above, the six degrees of freedom pose estimate8may be determined using any number of pose estimation approaches such as, for example, the perspective-n-point algorithm. The method200may then proceed to block208.

In block208, the control module40determines the estimated point of intersection W (FIG.2) representing where the line-of-sight L intersects with the six degrees of freedom pose estimate8of the target object12. The method200may then proceed to block210.

In block210, the control module40determines the estimated distance D measured between the range-sensing device44and the estimated point of intersection W. The method200may then proceed to block212.

In block212, the control module40calculates the absolute error associated with the six degrees of freedom pose estimate8of the target object12based on a difference between the actual distance and the estimated distance. The method200may then proceed to decision block214.

In decision block214, the revised six degree of freedom estimate is determined based on either the absolute error alone or, in the alternative, based on the absolute error and the reprojection error. If the control module40determines the revised six degree of freedom pose estimate is determined based on the absolute error alone, then the method proceeds to block216.

In block216, the control module40calculates the minimum value of the absolute error. As explained above, the control module40calculates the absolute error associated with the six degrees of freedom pose estimate8of the target object12based on a difference between the actual distance d and the estimated distance D and is expressed in Equation 1. The method200may then proceed to block218.

In block218, the control module40calculates the revised six degrees of freedom pose estimate, where the revised six degree of freedom pose estimate produces the minimum value of the absolute error. The method200may then terminate.

Referring back to decision block214, if the revised six degrees of freedom pose estimate is not determined based on the absolute error alone, then the method200proceeds to block220, which is shown inFIG.4B. Specifically, if the control module40determines the revised six degree of freedom pose estimate based on both the absolute error and the reprojection error then the method200proceeds to block220.

In block220, the control module40determines the reprojection error introduced by the six degrees of freedom pose estimate8of the target object12. As explained above, the reprojection error represents the difference between the plurality of two-dimensional pixel positions and the plurality of two-dimensional keypoints60shown inFIG.3. It is to be appreciated that a process flow diagram for determining the reprojection error is shown inFIG.4C. The method200may then proceed to block222.

In block222, the control module40determines the minimum value of the weighted sum, where the weighted sum combines the absolute error and the reprojection error together. The minimum value of the weighted sum may be determined using a variety of different approaches such as, for example, the Levenberg-Marquardt algorithm. The method200may then proceed to block224.

In block224, the control module40calculates the revised six degrees of freedom pose estimate, where the revised six degrees of freedom pose estimate produces the minimum value of the weighted sum. In an embodiment, the method200may then proceed to block226.

In block226, in one embodiment, the disclosed system10includes the extendable arm38(seen inFIG.1as the boom assembly20and inFIG.2). Accordingly, in block226, in response to determining the revised six degrees of freedom pose estimate, the control module40determines a position and an orientation of the extendable arm38based on the revised six degrees of freedom pose estimate. The method200may then terminate.

Referring now toFIG.4C, a process flow diagram illustrating a method250for determining the reprojection error is now described. Referring toFIGS.1,3, and4C, the method250begins at block252. In block252, the control module40detects the plurality of three-dimensional keypoints62that correspond to the target object12based on the image data captured by the camera42. The method250may then proceed to block254.

The block254, a deep neural network predicts the corresponding two-dimensional keypoint60for each of the plurality of three-dimensional keypoints62. The method250may then proceed to block256.

In block256, the control module40aligns the plurality of three-dimensional keypoints62with the plurality of two-dimensional keypoints60. The method250may then proceed to block258.

In block258, the control module40predicts the six degrees of freedom pose estimate8based on the three-dimensional keypoints62. The method250may then proceed to block260.

In block260, the control module40determines the plurality of two-dimensional pixel positions by projecting the plurality of three-dimensional keypoints62into two-dimensional space. The method250may then proceed to block262.

In block262, the control module40determines the difference between a plurality of two-dimensional pixel positions and the plurality of two-dimensional keypoints60, where the difference between the plurality of two-dimensional pixel positions and the plurality of two-dimensional keypoints60represent the reprojection error. The method250may then terminate.

Referring generally to the figures, the disclosed system provides various technical effects and benefits. Specifically, the disclosed system utilizes a single one-dimensional measurement from the range-sensing device for refining the six degrees of freedom pose estimate as opposed to a two-dimensional scan or, alternatively, a three-dimensional depth map. Accordingly, the disclosed system does not require significant processing and memory allocation requirements or a laser range finder having precisely manufactured moving parts like some conventional systems currently available. Additionally, the disclosed system does not require specialized calibration patterns or correspondence markers during the refinement process, unlike some conventional systems currently available as well.

Referring toFIG.5, the control module40ofFIG.1may be implemented on one or more computer devices or systems, such as exemplary computer system1030. The computer system1030includes a processor1032, a memory1034, a mass storage memory device1036, an input/output (I/O) interface1038, and a Human Machine Interface (HMI)1040. The computer system1030is operatively coupled to one or more external resources1042via the network1026or I/O interface1038. External resources may include, but are not limited to, servers, databases, mass storage devices, peripheral devices, cloud-based network services, or any other suitable computer resource that may be used by the computer system1030.

The processor1032includes one or more devices selected from microprocessors, micro-controllers, digital signal processors, microcomputers, central processing units, field programmable gate arrays, programmable logic devices, state machines, logic circuits, analog circuits, digital circuits, or any other devices that manipulate signals (analog or digital) based on operational instructions that are stored in the memory1034. Memory1034includes a single memory device or a plurality of memory devices including, but not limited to, read-only memory (ROM), random access memory (RAM), volatile memory, non-volatile memory, static random-access memory (SRAM), dynamic random-access memory (DRAM), flash memory, cache memory, or any other device capable of storing information. The mass storage memory device1036includes data storage devices such as a hard drive, optical drive, tape drive, volatile or non-volatile solid-state device, or any other device capable of storing information.

The processor1032operates under the control of an operating system1046that resides in memory1034. The operating system1046manages computer resources so that computer program code embodied as one or more computer software applications, such as an application1048residing in memory1034, may have instructions executed by the processor1032. In an alternative example, the processor1032may execute the application1048directly, in which case the operating system1046may be omitted. One or more data structures1049also reside in memory1034, and may be used by the processor1032, operating system1046, or application1048to store or manipulate data.

The I/O interface1038provides a machine interface that operatively couples the processor1032to other devices and systems, such as the network1026or external resource1042. The application1048thereby works cooperatively with the network1026or external resource1042by communicating via the I/O interface1038to provide the various features, functions, applications, processes, or modules comprising examples of the disclosure. The application1048also includes program code that is executed by one or more external resources1042, or otherwise rely on functions or signals provided by other system or network components external to the computer system1030. Indeed, given the nearly endless hardware and software configurations possible, persons having ordinary skill in the art will understand that examples of the disclosure may include applications that are located externally to the computer system1030, distributed among multiple computers or other external resources1042, or provided by computing resources (hardware and software) that are provided as a service over the network1026, such as a cloud computing service.

The HMI1040is operatively coupled to the processor1032of computer system1030in a known manner to allow a user to interact directly with the computer system1030. The HMI1040may include video or alphanumeric displays, a touch screen, a speaker, and any other suitable audio and visual indicators capable of providing data to the user. The HMI1040also includes input devices and controls such as an alphanumeric keyboard, a pointing device, keypads, pushbuttons, control knobs, microphones, etc., capable of accepting commands or input from the user and transmitting the entered input to the processor1032.

A database1044may reside on the mass storage memory device1036and may be used to collect and organize data used by the various systems and modules described herein. The database1044may include data and supporting data structures that store and organize the data. In particular, the database1044may be arranged with any database organization or structure including, but not limited to, a relational database, a hierarchical database, a network database, or combinations thereof. A database management system in the form of a computer software application executing as instructions on the processor1032may be used to access the information or data stored in records of the database1044in response to a query, where a query may be dynamically determined and executed by the operating system1046, other applications1048, or one or more modules.

The description of the present disclosure is merely exemplary in nature and variations that do not depart from the gist of the present disclosure are intended to be within the scope of the present disclosure. Such variations are not to be regarded as a departure from the spirit and scope of the present disclosure.