DETERMINING A DISTANCE BETWEEN A CAMERA AND A HITCH BALL

Abstract

A system for determining a distance between a camera and a hitch ball of a vehicle. The system includes a camera configured to be mounted on a vehicle and an electronic processor connected to the camera. The electronic processor is configured to determine a first position and a second position associated with a point on a trailer, determine an angle of rotation between the first position and the second position, and using the first position, the second position, and the angle of rotation, determine a null space of a matrix. In one example, the matrix is

[00001]$A=\left[\begin{array}{ccccc}1& 0& 0& -{\mathrm{cu}}_{\mathrm{xo}}& {\mathrm{cl}}_x\\ 0& 1& 0& -{\mathrm{cu}}_{y0}& {\mathrm{cl}}_y\\ 0& 0& 0& {\mathrm{cu}}_{z0}& -{\mathrm{cl}}_z\\ 0& 0& 1& \frac{-{\mathrm{cu}}_{z0}}{{\mathrm{cu}}_{z1}}{\mathrm{cu}}_{x1}& {\mathrm{cl}}_x\\ -\cos \delta & \sin \delta & 1& 0& 0\end{array}\right]$

and the null space of the matrix includes a value of a distance (d.sub.l) between the camera and the hitch ball. The electronic processor is further configured to perform an autonomous or semi-autonomous vehicle operation based on the distance (d.sub.l) between the camera and the hitch ball.

Claims

1. A system for determining a distance between a camera and a hitch ball of a vehicle, the system comprising: a camera configured to be mounted on a vehicle; and an electronic processor connected to the camera, the electronic processor configured to determine a first position and a second position associated with a point on a trailer; determine an angle of rotation between the first position and the second position; using the first position, the second position, and the angle of rotation, determine a null space of a matrix, wherein the matrix is defined as $A=\left[\begin{array}{ccccc}1& 0& 0& -{\mathrm{cu}}_{\mathrm{xo}}& {\mathrm{cl}}_x\\ 0& 1& 0& -{\mathrm{cu}}_{y0}& {\mathrm{cl}}_y\\ 0& 0& 0& {\mathrm{cu}}_{z0}& -{\mathrm{cl}}_z\\ 0& 0& 1& \frac{-{\mathrm{cu}}_{z0}}{{\mathrm{cu}}_{z1}}{\mathrm{cu}}_{x1}& {\mathrm{cl}}_x\\ -\cos \delta & \sin \delta & 1& 0& 0\end{array}\right]$ and the null space of the matrix includes a value of a distance (d.sub.l) between the camera and the hitch ball; and perform an autonomous or semi-autonomous vehicle operation based on the distance (d.sub.l) between the camera and the hitch ball.

2. The system according to claim 1, wherein the electronic processor is further configured to determine a position of the hitch ball by multiplying a third unit vector (cl.sub.xcl.sub.ycl.sub.z) by the distance between the camera and the hitch ball.

3. The system according to claim 1, wherein the camera is located at a horizontal center of a rear end of the vehicle.

4. The system according to claim 1, wherein the autonomous or semi-autonomous vehicle operation includes at least one selected from the group comprising parking the trailer, maneuvering the trailer around a corner while the vehicle is moving forward, maneuvering the trailer around a corner while the vehicle is moving backward, moving the trailer backward in a straight line, and making a left or right 90-degree turn with the trailer while the vehicle is driving backward.

5. The system according to claim 1, wherein the point on the trailer includes one selected from the group comprising a point on a body of the trailer and a point on a trailer bar.

6. The system according to claim 1, wherein a null space of the matrix is
x=[t.sub.x0t.sub.y0t.sub.x1d.sub.0d.sub.l].

7. A method for determining a distance between a camera and a hitch ball of a vehicle, the method comprising: breaking a first vector (t.sub.0) from a hitch ball mounted to a vehicle to a first position and a second vector (t.sub.1) from the hitch ball to a second position into three-dimensional coordinates to determine a first system of equations, wherein the first vector (t.sub.0) from the hitch ball to the first position is a first unit vector (cu.sub.0) from a camera mounted on the vehicle to the first position multiplied by a first distance (d.sub.0) between the camera and the first position minus a third vector (L) from the camera to the hitch ball and the second vector (t.sub.1) between the hitch ball and the second position is a second unit vector (cu.sub.1) from the camera to the second position multiplied by a second distance (d.sub.1) between the camera and the second position minus the third vector (L) from the camera to the hitch ball; based on an assumption that the first position and the second position are at the same height, determining d.sub.1=(d.sub.0*cu.sub.z0)/cu.sub.z1; substituting d.sub.1 in the first system of equations for (d.sub.0*cu.sub.z0)/cu.sub.z1 to determine a second system of equations; determining a third system of equations by adding an equation to the second system of equations, wherein the equation is derived from a rotation matrix describing movement of the point from the first position to the second position by the angle of rotation; based on the third vector (L) from the camera to the hitch ball being a third unit vector (cl.sub.xcl.sub.ycl.sub.z) multiplied by the distance (d.sub.l) between the camera and the hitch ball, substituting L.sub.x in the third system of equations for d.sub.lcl.sub.x, L.sub.y in the third system of equations for d.sub.lcl.sub.y, and L.sub.z in the third system of equations for d.sub.lcl.sub.z to determine a fourth system of equations; based on an assumption that the hitch ball is at the same height as the first position and the second position, substituting t.sub.z with 0 in the fourth system of equations and re-arranging equations in the fourth system of equations to determine a fifth system of equations; and based on the fifth system of equations, determine a distance between a camera and a hitch ball of a vehicle.

8. The method according to claim 7, wherein the first position and the second position are associated with a point on a trailer.

9. The method according to claim 7, the method further comprising: determining the third unit vector (cl.sub.xcl.sub.ycl.sub.z) based on a focal length associated with the camera and an offset of the camera from a horizontal center of a rear end of the vehicle; and determining a position of the hitch ball by multiplying the third unit vector (cl.sub.xcl.sub.ycl.sub.z) by the distance (d.sub.l) between the camera and the hitch ball.

10. The method according to claim 7, wherein the first system of equations is:
t.sub.x0=d.sub.0*cu.sub.x0−L.sub.x
t.sub.y0=d.sub.0*cu.sub.y0−L.sub.y
t.sub.z0=d.sub.0*cu.sub.z0−L.sub.z
t.sub.x1=d.sub.1*cu.sub.x1−L.sub.x
t.sub.y1=d.sub.1*cu.sub.y1−L.sub.y
t.sub.z1=d.sub.1*cu.sub.z1−L.sub.z the second system of equations is:
t.sub.x0=d.sub.0cu.sub.x0−L.sub.x
t.sub.y0=d.sub.0cu.sub.y0−L.sub.y
t.sub.z=d.sub.0cu.sub.z0−L.sub.z
t.sub.x1=((d.sub.0cu.sub.z0)/cu.sub.z1)cu.sub.x1−L.sub.x the third system of equations is:
t.sub.x0=d.sub.0cu.sub.x0−L.sub.x
t.sub.y0=d.sub.0cu.sub.y0−L.sub.y
t.sub.z=d.sub.0cu.sub.z0−L.sub.z
t.sub.x1=((d.sub.0cu.sub.z0)/cu.sub.z1)cu.sub.x1−L.sub.x
t.sub.x1=t.sub.x0 cos δ−t.sub.y0 sin δ the fourth system of equations is:
t.sub.x0−d.sub.0cu.sub.x0=−d.sub.lcl.sub.x
t.sub.y0−d.sub.0cu.sub.y0=−d.sub.lcl.sub.y
t.sub.z=d.sub.0cu.sub.z0−d.sub.lcl.sub.z
t.sub.x1−((d.sub.0cu.sub.z0)/cu.sub.z1)cu.sub.x1=−d.sub.lcl.sub.x
t.sub.x1=t.sub.x0 cos δ−t.sub.y0 sin δ and, the fifth system of equations is:
t.sub.x0−d.sub.0cu.sub.x0+d.sub.lcl.sub.x=0
t.sub.y0−d.sub.0cu.sub.y0+d.sub.lcl.sub.y=0
d.sub.0cu.sub.z0−d.sub.lcl.sub.z=0
t.sub.x1−((d.sub.0cu.sub.z0)/cu.sub.z1)cu.sub.x1+d.sub.lcl.sub.x=0
t.sub.x0 cos δ+t.sub.y0 sin δ+t.sub.x1=0.

11. The method according to claim 7, further comprising a determining a matrix based on the fifth system of equations, wherein the matrix is $A=\left[\begin{array}{ccccc}1& 0& 0& -{\mathrm{cu}}_{\mathrm{xo}}& {\mathrm{cl}}_x\\ 0& 1& 0& -{\mathrm{cu}}_{y0}& {\mathrm{cl}}_y\\ 0& 0& 0& {\mathrm{cu}}_{z0}& -{\mathrm{cl}}_z\\ 0& 0& 1& \frac{-{\mathrm{cu}}_{z0}}{{\mathrm{cu}}_{z1}}{\mathrm{cu}}_{x1}& {\mathrm{cl}}_x\\ -\cos \delta & \sin \delta & 1& 0& 0\end{array}\right].$

12. A method for performing a vehicle operation based on a distance between a camera and a hitch ball of a vehicle, the method comprising: determining, using a camera mounted to the vehicle, a first position and a second position associated with a point on a trailer; determining an angle of rotation between the first position and the second position; using the first position, the second position, and the angle of rotation, determining a null space of a matrix, wherein the matrix is $A=\left[\begin{array}{ccccc}1& 0& 0& -{\mathrm{cu}}_{\mathrm{xo}}& {\mathrm{cl}}_x\\ 0& 1& 0& -{\mathrm{cu}}_{y0}& {\mathrm{cl}}_y\\ 0& 0& 0& {\mathrm{cu}}_{z0}& -{\mathrm{cl}}_z\\ 0& 0& 1& \frac{-{\mathrm{cu}}_{z0}}{{\mathrm{cu}}_{z1}}{\mathrm{cu}}_{x1}& {\mathrm{cl}}_x\\ -\cos \delta & \sin \delta & 1& 0& 0\end{array}\right]$ and the null space of the matrix includes a value of a distance (d.sub.l) between the camera and the hitch ball; and performing an autonomous or semi-autonomous vehicle operation based on the distance (d.sub.l) between the camera and the hitch ball.

13. The method according to claim 12, wherein the camera is located at a horizontal center of a rear end of the vehicle.

14. The method according to claim 12, wherein the autonomous or semi-autonomous vehicle operation includes at least one selected from the group comprising parking the trailer, maneuvering the trailer around a corner while the vehicle is moving forward, maneuvering the trailer around a corner while the vehicle is moving backward, moving the trailer backward in a straight line, and making a left or right 90-degree turn with the trailer while the vehicle is driving backward.

15. The method according to claim 12, wherein the point on the trailer includes one selected from the group comprising a point on a body of the trailer and a point on a trailer bar.

16. The method according to claim 12, the method comprising determining the matrix by breaking a first vector (t.sub.0) from the hitch ball to the first position and a second vector (t.sub.1) from the hitch ball to the second position into three-dimensional coordinates to determine a first system of equations, wherein the first vector (t.sub.0) from the hitch ball to the first position is a first unit vector (cu.sub.0) from the camera to the first position multiplied by a first distance (d.sub.0) between the camera and the first position minus a third vector (L) from the camera to the hitch ball and the second vector (t.sub.1) between the hitch ball and the second position is a second unit vector (cu.sub.1) from the camera to the second position multiplied by a second distance (d.sub.1) between the camera and the second position minus the third vector (L) from the camera to the hitch ball; based on an assumption that the first position and the second position are at the same height, determining d.sub.1=(d.sub.0*cu.sub.z0)/cu.sub.z1; substituting d.sub.1 in the first system of equations for (d.sub.0*cu.sub.z0)/cu.sub.z1 to determine a second system of equations; determining a third system of equations by adding an equation to the second system of equations, wherein the equation is derived from a rotation matrix describing movement of the point from the first position to the second position by the angle of rotation; based on the third vector (L) from the camera to the hitch ball being a third unit vector (cl.sub.xcl.sub.ycl.sub.z) multiplied by the distance (d.sub.l) between the camera and the hitch ball, substituting L.sub.x in the third system of equations for d.sub.lcl.sub.x, L.sub.y in the third system of equations for d.sub.lcl.sub.y, and L.sub.z in the third system of equations for d.sub.lcl.sub.z to determine a fourth system of equations; based on an assumption that the hitch ball is at the same height as the first position and the second position, substituting t.sub.z with 0 in the fourth system of equations and re-arranging equations in the fourth system of equations to determine a fifth system of equations; and based on the fifth system of equations, determining the matrix for determining a distance between a camera and a hitch ball of a vehicle.

17. The method according to claim 16, the method further comprising: determining the third unit vector (cl.sub.xcl.sub.ycl.sub.z) based on a focal length associated with the camera and an offset of the camera from a horizontal center of a rear end of the vehicle; and determining a position of the hitch ball by multiplying the third unit vector (cl.sub.xcl.sub.ycl.sub.z) by the distance (d.sub.l) between the camera and the hitch ball.

18. The method according to claim 16, wherein the first system of equations is:
t.sub.x0=d.sub.0*cu.sub.x0−L.sub.x
t.sub.y0=d.sub.0*cu.sub.y0−L.sub.y
t.sub.z0=d.sub.0*cu.sub.z0−L.sub.z
t.sub.x1=d.sub.1*cu.sub.x1−L.sub.x
t.sub.y1=d.sub.1*cu.sub.y1−L.sub.y
t.sub.z1=d.sub.1*cu.sub.z1−L.sub.z the second system of equations is:
t.sub.x0=d.sub.0cu.sub.x0−L.sub.x
t.sub.y0=d.sub.0cu.sub.y0−L.sub.y
t.sub.z=d.sub.0cu.sub.z0−L.sub.z
t.sub.x1=((d.sub.0cu.sub.z0)/cu.sub.z1)cu.sub.x1−L.sub.x the third system of equations is:
t.sub.x0=d.sub.0cu.sub.x0−L.sub.x
t.sub.y0=d.sub.0cu.sub.y0−L.sub.y
t.sub.z=d.sub.0cu.sub.z0−L.sub.z
t.sub.x1=((d.sub.0cu.sub.z0)/cu.sub.z1)cu.sub.x1−L.sub.x
t.sub.x1=t.sub.x0 cos δ−t.sub.y0 sin δ the fourth system of equations is:
t.sub.x0−d.sub.0cu.sub.x0=−d.sub.lcl.sub.x
t.sub.y0−d.sub.0cu.sub.y0=−d.sub.lcl.sub.y
t.sub.z=d.sub.0cu.sub.z0−d.sub.lcl.sub.z
t.sub.x1−((d.sub.0cu.sub.z0)/cu.sub.z1)cu.sub.x1=−d.sub.lcl.sub.x
t.sub.x1=t.sub.x0 cos δ−t.sub.y0 sin δ and, the fifth system of equations is:
t.sub.x0−d.sub.0cu.sub.x0+d.sub.lcl.sub.x=0
t.sub.y0−d.sub.0cu.sub.y0+d.sub.lcl.sub.y=0
d.sub.0cu.sub.z0−d.sub.lcl.sub.z=0
t.sub.x1−((d.sub.0cu.sub.z0)/cu.sub.z1)cu.sub.x1+d.sub.lcl.sub.x=0
t.sub.x0 cos δ+t.sub.y0 sin δ+t.sub.x1=0.

19. The method according to claim 12, wherein the null space of the matrix is
x=[t.sub.x0t.sub.y0t.sub.x1d.sub.0d.sub.l].

Description

BRIEF DESCRIPTION OF THE DRAWINGS

[0007] FIG. 1 is a block diagram of a system for determining a distance between a camera and a hitch ball according to one embodiment.

[0008] FIG. 2 illustrates a flow chart of a method for determining a distance between a camera and a hitch ball according to one embodiment.

[0009] FIG. 3 is an illustrative example of the geometric relationships between components of the system of FIG. 1 at two positions.

[0010] FIG. 4A and FIG. 4B illustrate an example flow chart of a method for determining a matrix for determining a distance between a camera and a hitch ball of a vehicle.

DETAILED DESCRIPTION

[0011] Before any embodiments are explained in detail, it is to be understood that this disclosure is not intended to be limited in its application to the details of construction and the arrangement of components set forth in the following description or illustrated in the following drawings. Embodiments are capable of other configurations and of being practiced or of being carried out in various ways.

[0012] A plurality of hardware and software based devices, as well as a plurality of different structural components may be used to implement various embodiments. In addition, embodiments may include hardware, software, and electronic components or modules that, for purposes of discussion, may be illustrated and described as if the majority of the components were implemented solely in hardware. However, one of ordinary skill in the art, and based on a reading of this detailed description, would recognize that, in at least one embodiment, the electronic based aspects of the invention may be implemented in software (for example, stored on non-transitory computer-readable medium) executable by one or more processors. For example, “control units” and “controllers” described in the specification can include one or more electronic processors, one or more memory modules including non-transitory computer-readable medium, one or more communication interfaces, one or more application specific integrated circuits (ASICs), and various connections (for example, a system bus) connecting the various components. It should be understood that although certain drawings illustrate hardware and software located within particular devices, these depictions are for illustrative purposes only. In some embodiments, the illustrated components may be combined or divided into separate software, firmware and/or hardware. For example, instead of being located within and performed by a single electronic processor, logic and processing may be distributed among multiple electronic processors. Regardless of how they are combined or divided, hardware and software components may be located on the same computing device or may be distributed among different computing devices connected by one or more networks or other suitable communication links.

[0013] FIG. 1 illustrates one example of a system 100 for determining a distance between a camera and a hitch ball. The system 100 includes a vehicle 105 and a trailer 110. The vehicle 105, although illustrated as a four-wheeled vehicle, may encompass various types and designs of vehicles. For example, the vehicle 105 may be an automobile, a motorcycle, a truck, a bus, a semi-tractor, and others. The trailer 110 although illustrated as a four-wheeled trailer, may encompass various types and designs of trailers. For example, the trailer 110 may include two wheels instead of four or the trailer 110 may be shaped like a rectangle rather than a square. The vehicle 105 and the trailer 110 are connected via a hitch ball 115 that is mounted to the vehicle 105 and a trailer bar 120 that extends from the trailer 110. In some embodiments, the hitch ball 115 is connected to the rear end of the vehicle 105 via a hitch ball mount 125. In some embodiments, the hitch ball 115 is located at the horizontal center of the rear end of the vehicle 105.

[0014] In the example shown in FIG. 1, the vehicle 105 includes an electronic controller 130 and a camera 135. In the example shown, the electronic controller 130 includes a plurality of electrical and electronic components that provide power, operation control, and protection to the components and modules within the electronic controller 130. The electronic controller 130 includes, among other things, an electronic processor 140 (such as a programmable electronic microprocessor, microcontroller, or similar device), a memory 145 (for example, a non-transitory, machine readable medium), and an interface 150. The electronic processor 140 is communicatively connected to the memory 145 and the interface 150. In some embodiments, the electronic processor 140, in coordination with software stored in the memory 145 and information from the sensors, is configured to implement, among other things, the methods described herein.

[0015] The electronic controller 130 may be implemented in several independent controllers (for example, programmable electronic controllers) each configured to perform specific functions or sub-functions. Additionally, the electronic controller 130 may contain sub-modules that include additional electronic processors, memory, or application specific integrated circuits (ASICs) for handling communication functions, processing of signals, and application of the methods listed below. In other embodiments, the electronic controller 130 includes additional, fewer, or different components.

[0016] As illustrated in FIG. 1, the electronic processor 140 may communicate with the camera 135 via the interface 150. The camera 135 may be configured to capture images, video, or both. In some embodiments, the camera 135 is located at the rear end of the vehicle 105 and aligned with the hitch ball 115. In other words, the camera 135 and the hitch ball 115 are located at the same horizontal point on the rear end of the vehicle 105. In other embodiments, the camera 135 is horizontally offset from the hitch ball 115. While not illustrated in FIG. 1, in some embodiments, the vehicle 105 includes a speed sensor and a steering column sensor which communicate with the electronic processor 140 via the interface 150. The speed sensor may be mounted on a wheel axel and configured to measure the speed of the vehicle 105. The steering column sensor may be mounted on the steering column and configured to determine the rotational position of the steering column.

[0017] FIG. 2 illustrates an example flowchart of a method 200 for determining a distance between a camera and a hitch ball. The method 200 begins at step 205 when the electronic processor 140 determines or receives a first position and a second position associated with a point on the trailer 110. In some embodiments, the point is a point on a body of the trailer 110. In other embodiments, the point is a point on a trailer bar 120 that connects the trailer body to the hitch ball 115. In some embodiments, the electronic processor 140 determines a first position and a second position associated with a point on the trailer 110 based on a first image and a second image received from the camera 135. In some embodiments, the camera 135 captures the first image and then captures the second image after the electronic processor 140 determines a change in the position of the steering column has occurred and the wheels of the vehicle 105 have rotated. In some embodiments, when the electronic processor 140 receives a first image, the electronic processor 140 outputs a prompt to a driver to turn the steering wheel of the vehicle 105 to the left or to the right and to drive the vehicle 105 forward or backward. In some embodiments, the prompt is in the form of an audio output via a speaker included in the vehicle 105, a visual output via a screen included in the vehicle 105, a combination of the foregoing, or the like. In some embodiments the electronic processor 140 uses image feature analysis to determine that the point is located at the first position in the first image and the second position in the second image. The electronic processor 140 may also use image feature analysis to determine that the second position is horizontally offset from the first position. FIG. 3 is an illustrative example of the geometric relationships between components of the system of FIG. 1 at two positions (the first position and the second position). In FIG. 3 the first position is illustrated as pt.sub.0 and the second position is illustrated as pt.sub.1.

[0018] At step 210, the electronic processor 140 determines or receives an angle of rotation between the first position (pt.sub.0) and the second position (pt.sub.1). In some embodiments, the electronic processor 140 uses image feature analysis to determine the angle of rotation. In other embodiments, the electronic processor 140 performs a calculation based on a change in steering angle (a change in the angular position of the steering column) and a change in velocity of the vehicle 105 from a time at which the first image is captured and to a time at which the second image is captured to determine the angle of rotation. In FIG. 3 the angle of rotation is illustrated as δ.

[0019] At step 215, using the first position, the second position, and the angle of rotation, the electronic processor 140 determines the null space of a matrix. In some embodiments, the matrix is defined as

[00004]$A=\left[\begin{array}{ccccc}1& 0& 0& -{\mathrm{cu}}_{\mathrm{xo}}& {\mathrm{cl}}_x\\ 0& 1& 0& -{\mathrm{cu}}_{y0}& {\mathrm{cl}}_y\\ 0& 0& 0& {\mathrm{cu}}_{z0}& -{\mathrm{cl}}_z\\ 0& 0& 1& \frac{-{\mathrm{cu}}_{z0}}{{\mathrm{cu}}_{z1}}{\mathrm{cu}}_{x1}& {\mathrm{cl}}_x\\ -\cos \delta & \sin \delta & 1& 0& 0\end{array}\right]$

[0020] In some embodiments, the null space is:

x=[t.sub.x0t.sub.y0t.sub.x1d.sub.0d.sub.l]

[0021] In some embodiments, the null space includes a value of the distance (d.sub.l) between the camera 135 and the hitch ball 115 and, by determining the null space, the electronic processor 140 determines the distance between the camera 135 and the hitch ball 115.

[0022] FIG. 4A and FIG. 4B illustrate an example method 400 of determining a matrix for determining a distance between a camera and a hitch ball of a vehicle (for example, the matrix described in step 215 of the method 200).

[0023] A first vector (t.sub.0) from the hitch ball 115 to the first position (pt.sub.0) can be described in terms of vector addition as t.sub.0=c.sub.0−L. As illustrated in FIG. 3, c.sub.0 is a vector from the camera 135 to the first position (pt.sub.0) and L is a third vector from the camera 135 to the hitch ball 115. A second vector from the hitch ball 115 to the second position (pt.sub.1) can be described in terms of vector addition as t.sub.1=c.sub.1−L. As illustrated in FIG. 3, c.sub.1 is the vector from the camera 135 to the second position (pt.sub.1). The vector (c.sub.0) from the camera 135 to the first position (pt.sub.0) can be described as follows: c.sub.0=cu.sub.0*d.sub.0. cu.sub.0 is a first unit vector from the camera 135 to the first position (pt.sub.0) and d.sub.0 is an unknown first distance between the camera 135 and the first position (pt.sub.0). The vector (c.sub.1) from the camera 135 to the second position (pt.sub.1) can be described as follows: c.sub.1=cu.sub.1*d.sub.1. cu.sub.1 is a second unit vector from the camera 135 to the second position (pt.sub.1) and d.sub.1 is an unknown second distance between the camera 135 and the second position (pt.sub.1). Substituting c.sub.0 for cu.sub.0*d.sub.0 in the equation t.sub.0=c.sub.0−L and c.sub.1 for cu.sub.1*d.sub.1 in the equation t.sub.1=c.sub.1−L results in the following two equations:

t.sub.0=d.sub.0*cu.sub.0−L

t.sub.1=d.sub.1*cu.sub.1−L

[0024] In other words, the first vector (t.sub.0) from the hitch ball 115 to the first position (pt.sub.0) may be described as the first unit vector (cu.sub.0) from the camera 135 to the first position (pt.sub.0) multiplied by the first distance (d.sub.0) between the camera 135 and the first position (pt.sub.0) minus the third vector (L) from the camera 135 to the hitch ball 115. The second vector (t.sub.1) between the hitch ball 115 and the second position (pt.sub.1) may be described as the second unit vector (cu.sub.1) from the camera 135 to the second position (pt.sub.1) multiplied by the second distance (d.sub.1) between the camera 135 and the second position (pt.sub.1) minus the third vector (L) from the camera 135 to the hitch ball 115. In some embodiments, the first unit vector (cu.sub.0) and the second unit vector (cu.sub.1) may be determined using the focal length of the camera 135.

[0025] In some embodiments, the method 400 begins at step 405, when the first vector (t.sub.0) from the hitch ball 115 to the first position (pt.sub.0) and the second vector (t.sub.1) from the hitch ball 115 to the second position (pt.sub.1) are broken into three-dimensional coordinates to determine a first system of equations. In some embodiments, the first system of equations is:

t.sub.x0=d.sub.0*cu.sub.x0−L.sub.x

t.sub.y0=d.sub.0*cu.sub.y0−L.sub.y

t.sub.z0=d.sub.0*cu.sub.z0−L.sub.z

t.sub.x1=d.sub.1*cu.sub.x1−L.sub.x

t.sub.y1=d.sub.1*cu.sub.y1−L.sub.y

t.sub.z1=d.sub.1*cu.sub.z1−L.sub.z

[0026] At step 410, based on an assumption that the first position and the second position are at the same height, the method 400 includes determining d.sub.1=(d.sub.0*cu.sub.z0)/cu.sub.z1. d.sub.1=(d.sub.0*cu.sub.z0)/cu.sub.z1 is determined because if t.sub.z0=t.sub.z1, then, based on the first system of equations, d.sub.1*cu.sub.z1−L.sub.z=d.sub.0*cu.sub.z0−L.sub.z.

[0027] At step 415, d.sub.1 is substituted in the first system of equations for (d.sub.0*cu.sub.z0)/cu.sub.z1 to determine a second system of equations. In some embodiments, the second system of equations is:

t.sub.x0=d.sub.0cu.sub.x0−L.sub.x

t.sub.y0=d.sub.0cu.sub.y0−L.sub.y

t.sub.z=d.sub.0cu.sub.z0−L.sub.z

t.sub.x1=((d.sub.0cu.sub.z0)/cu.sub.z1)cu.sub.x1−L.sub.x

[0028] At step 420, a third system of equations is determined by adding an equation to the second system of equations. The equation that is being added to the second system of equations is derived from a rotation matrix describing movement of the point from the first position (pt.sub.0) to the second position (pt.sub.1) by the angle of rotation (β). In some embodiments, the rotation matrix is

[00005]$\mathrm{Rot}=\left[\begin{array}{ccc}\cos \delta & -\sin \delta & 0\\ \sin \delta & \cos \delta & 0\\ 0& 0& 1\end{array}\right]$

[0029] In the above rotation matrix, the first row corresponds to the x component of the second vector (t.sub.x1), the second row corresponds to the y component of the second vector (t.sub.y1), and the third row corresponds to the z component of the second vector (t.sub.z1). Each column in the matrix corresponds to an unknown variable of the first vector (t.sub.0). For example, the first column corresponds to the variable t.sub.x0, the second column corresponds to the variable t.sub.y0, and the third column corresponds to the variable t.sub.z0. Each column includes the value that the variable the column corresponds to is multiplied by to describe second vector (t.sub.1).

[0030] The second vector (t.sub.1) may be described in terms of the rotation matrix as t.sub.1=Rot*t.sub.0. By reducing the equation t.sub.1=Rot*t.sub.0 to the x component, the electronic processor 140 determines the equation is t.sub.x1=t.sub.x0 cos δ−t.sub.y0 sin δ. Therefore, in some embodiments, the third set of equations is:

t.sub.x0=d.sub.0cu.sub.x0−L.sub.x

t.sub.y0=d.sub.0cu.sub.y0−L.sub.y

t.sub.z=d.sub.0cu.sub.z0−L.sub.z

t.sub.x1=((d.sub.0cu.sub.z0)/cu.sub.z1)cu.sub.x1−L.sub.x

t.sub.x1=t.sub.x0 cos δ−t.sub.y0 sin δ

[0031] At step 425, based on the third vector (L) from the camera 135 to the hitch ball 115 being a third unit vector (cl.sub.xcl.sub.ycl.sub.z) multiplied by the distance (d.sub.l) between the camera 135 and the hitch ball 115, L.sub.x is substituted in the third system of equations for d.sub.lcl.sub.x, L.sub.y in the third system of equations for d.sub.lcl.sub.y, and L.sub.z in the third system of equations for d.sub.lcl.sub.z to determine a fourth system of equations. In some embodiments, the fourth system of equations is:

t.sub.x0−d.sub.0cu.sub.x0=−d.sub.lcl.sub.x

t.sub.y0−d.sub.0cu.sub.y0=−d.sub.lcl.sub.y

t.sub.z=d.sub.0cu.sub.z0−d.sub.lcl.sub.z

t.sub.x1−((d.sub.0cu.sub.z0)/cu.sub.z1)cu.sub.x1=−d.sub.lcl.sub.x

t.sub.x1=t.sub.x0 cos δ−t.sub.y0 sin δ

[0032] At step 430, based on an assumption that the hitch ball 115 is at the same height as the first position (pt.sub.0) and the second position (pt.sub.1), t.sub.z is substituted with 0 in the fourth system of equations and re-arranges the equations in the fourth system of equations to determine a fifth system of equations with five unknowns. In some embodiments, the five unknowns are t.sub.x0, t.sub.y0, t.sub.x1, d.sub.l, and d.sub.0. In some embodiments, the fifth system of equations is:

t.sub.x0−d.sub.0cu.sub.x0+d.sub.lcl.sub.x=0

t.sub.y0−d.sub.0cu.sub.y0+d.sub.lcl.sub.y=0

d.sub.0cu.sub.z0−d.sub.lcl.sub.z=0

t.sub.x1−((d.sub.0cu.sub.z0)/cu.sub.z1)cu.sub.x1+d.sub.lcl.sub.x=0

t.sub.x0 cos δ+t.sub.y0 sin δ+t.sub.x1=0

[0033] At step 435, based on the fifth system of equations, the matrix is determined. As described above, in some embodiments, the matrix is

[00006]$A=\left[\begin{array}{ccccc}1& 0& 0& -{\mathrm{cu}}_{\mathrm{xo}}& {\mathrm{cl}}_x\\ 0& 1& 0& -{\mathrm{cu}}_{y0}& {\mathrm{cl}}_y\\ 0& 0& 0& {\mathrm{cu}}_{z0}& -{\mathrm{cl}}_z\\ 0& 0& 1& \frac{-{\mathrm{cu}}_{z0}}{{\mathrm{cu}}_{z1}}{\mathrm{cu}}_{x1}& {\mathrm{cl}}_x\\ -\cos \delta & \sin \delta & 1& 0& 0\end{array}\right]$

[0034] In the above matrix, each row corresponds to an equation included in the fifth system of equations. The first equation included in the fifth system of equations corresponds to the first row in the matrix, the second equation included in the fifth system of equations corresponds to the second row in the matrix, and so on. Each column in the matrix corresponds to an unknown variable in the fifth system of equations. For example, the first column of the matrix corresponds to the variable t.sub.x0, the second column of the matrix corresponds to the variable t.sub.y0, the third column of the matrix corresponds to the variable t.sub.x1, the fourth column of the matrix corresponds to the variable d.sub.0, the fifth column of the matrix corresponds to the variable d.sub.l. Each column includes the value that the variable the column corresponds to is multiplied by in an equation. For example, in the first row, the first column includes ‘1’ because in the first equation in the fifth system of equations includes t.sub.x0, In the second row, the first column includes ‘0’ because in the second equation in the fifth system of equations does not include t.sub.x0. In the fifth row, the first column includes “−cos δ” because the fifth equation in the fifth system of equations includes −t.sub.x0 cos δ.

[0035] Returning to the method 200 included in FIG. 2, at step 220, the electronic processor 140 performs an autonomous or semi-autonomous vehicle operation based on the distance (d.sub.l) between the camera 135 and the hitch ball 115. In some embodiments, in order to perform the autonomous or semi-autonomous vehicle operation, the electronic processor 140 uses the distance (d.sub.l) to perform one or more calculations. For example, in some embodiments, the electronic processor 140 uses the distance (d.sub.l) to determine the position of the hitch ball 115. In some embodiments, the electronic processor 140 determines the position of the hitch ball 115 by multiplying the third unit vector (cl.sub.xcl.sub.ycl.sub.z) by the distance (d.sub.l) between the camera 135 and the hitch ball 115. The electronic processor 140 may determine the third unit vector (cl.sub.xcl.sub.ycl.sub.z) based on a focal length associated with the camera 135 and an offset of the camera 135 from the horizontal center of the rear end of the vehicle 105. In some embodiments, the electronic processor 140 may use the position of the hitch ball 115 to determine the position of the trailer 110.

[0036] In some embodiments, the autonomous or semi-autonomous vehicle operation is parking the trailer 110, maneuvering the trailer 110 around a corner while the vehicle 105 is moving forward, maneuvering the trailer 110 around a corner while the vehicle 105 is moving backward, moving the trailer 110 backward in a straight line (a straight back maneuver), making a 90-degree turn (left or right) with the trailer 110 while the vehicle 105 is driving backward, a combination of the foregoing, or the like.

[0037] Thus, the embodiments described herein provide, among other things, a system and a method for determining a distance between a camera and a hitch ball. Various features and advantages of the embodiments are set forth in the following claims.