3D POSITION ESTIMATION SYSTEM FOR TRAILER COUPLER
A system for locating a coupler of a trailer includes at least one camera positioned on a rear portion of a tow vehicle. A coupler detector module is constructed and arranged 1) to receive images of the coupler from the at least one camera and 2) to determining a two-dimensional (2D) pixel position of the coupler. A camera motion estimator module is constructed and arranged 1) to receive images from the at least one camera and data regarding motion of the tow vehicle and 2) determine a pose of the camera including a three-dimensional (3D) position and heading of the at least one camera. A coupler estimator module is constructed and arranged 1) to receive the pose of the camera and the 2D pixel position of the coupler and based thereon, 2) to determine an estimated 3D position of the coupler in real world coordinates.
1 . A method of locating a coupler of a trailer, the method comprising:
receiving, at a coupler detector module, images of the coupler from a camera positioned on a rear portion of a tow vehicle and in communication with the coupler detector module;
determining, in the coupler detector module, a two-dimensional (2D) pixel position of the coupler;
receiving, by a camera motion estimator module, images from the camera in communication with the camera motion estimator module, and data regarding motion of the tow vehicle;
determining, by the camera motion estimator module, a pose of the camera including a three-dimensional (3D) position and heading of the camera;
receiving, by a coupler estimator module, the pose of the camera and the 2D pixel position of the coupler; and
based on the pose of the camera and the 2D pixel position of the coupler, determining, by the coupler estimator module, an estimated 3D position of the coupler in real world coordinates.
2 . The method of claim 1 , wherein the estimated 3D position of the coupler in real world coordinates includes a longitude position, a latitude position, and a height position of the coupler.
3 . The method of claim 1 , further comprising:
sending, to a drive system of the tow vehicle, instructions causing the tow vehicle to autonomously drive along a path in a rearward direction towards the 3D position of the coupler.
4 . The method of claim 1 , wherein each of the determining steps is executed by a processor circuit associated with the respective module.
5 . The method of claim 1 , wherein the step of determining a 3D position of the coupler in real world coordinates comprises:
at various time intervals, forward projecting 2D points in image coordinates of a pair of rays of the camera, to 3D points in real world coordinates, with the intersection of each pair of rays defining the estimated 3D position of the coupler.
6 . The method of claim 5 , further comprising:
when noise is present causing the estimated 3D position of the coupler of one pair of rays of the camera to deviate from the estimated 3D position of the coupler of another pair of rays, filtering out the noise so as to obtain a converged, single estimated 3D position of the coupler.
7 . The method of claim 6 , wherein obtaining the converged, single estimated 3D position of the coupler includes using triangulation.
8 . The method of claim 6 , wherein the step of filtering includes using a Kalman filter.
9 . The method of claim 1 , wherein the step of receiving data regarding motion of the tow vehicle includes receiving at least acceleration and steering wheel angle data, and wheel encoder data.
10 . A system for locating a coupler of a trailer, the system comprising:
at least one camera positioned on a rear portion of a tow vehicle,
a coupler detector module constructed and arranged 1) to receive images of the coupler from the at least one camera and 2) to determining a two-dimensional (2D) pixel position of the coupler;
a camera motion estimator module, constructed and arranged 1) to receive images from the at least one camera and data regarding motion of the tow vehicle and 2) determine a pose of the camera including a three-dimensional (3D) position and heading of the at least one camera; and
a coupler estimator module constructed and arranged 1) to receive the pose of the camera and the 2D pixel position of the coupler and based thereon, 2) to determine an estimated 3D position of the coupler in real world coordinates.
11 . The system of claim 10 , wherein coupler estimator module is constructed and arranged to determine the estimated 3D position of the coupler in real world coordinates as a longitude position, a latitude position, and a height position of the coupler.
12 . The system of claim 10 , further comprising the tow vehicle and a drive system for the tow vehicle, the drive system being constructed and arranged to cause the tow vehicle to autonomously drive along a path in a rearward direction towards the 3D position of the coupler.
13 . The system of claim 10 , wherein each of the coupler detector module, the camera motion estimator module, and the coupler estimator module is associated with a processor circuit.
14 . The system of claim 10 , wherein the coupler estimator module is constructed and arranged, at various time intervals, to forward project 2D points in image coordinates of a pair of rays of the camera, to 3D points in real world coordinates, with the intersection of each pair of rays defining the estimated 3D position of the coupler.
15 . The system of claim 14 , further comprising a filter constructed and arranged when noise is present causing the estimated 3D position of the coupler of one pair of rays of the camera to deviate from the estimated 3D position of the coupler of another pair of rays, to filter out the noise so as to obtain a converged, single estimated 3D position of the coupler.
16 . The system of claim 15 , wherein the filter is a Kalman filter.
17 . The system of claim 10 , further comprising an acceleration and steering wheel angle sensor, and wheel encoder sensor, each associated with the camera motion estimator module to provide the data regarding motion of the tow vehicle.