Real-time trailer coupler location and tracking

Abstract

A method for detecting and locating a trailer coupler (212) of a trailer (200) is provided. The method includes receiving an image (143) from a camera (142) located on a rear portion of a towing vehicle (100) and determining a region of interest (300) within the image (143). The region of interest (300) includes a representation of the trailer coupler (212). The method includes determining a camera plane (310) and a road plane (320). In addition, the method includes determining a three-dimensional point cloud representing objects within the region of interest (300) and within the camera plane (310) and the road plane (320). The method also includes receiving sensor data from a sensor system (140) and determining a coupler position of the trailer coupler based on the 3D point cloud and the sensor data. The method also includes sending instructions to a driving system (110) to cause the towing vehicle (100) to autonomously drive along a path in a rearward direction toward the coupler position.

Description

Technical Field

[0001] The present disclosure relates to a method and apparatus for real-time coupler location and tracking. Background Art

[0002] Trailers are usually unpowered vehicles that are pulled by a powered towing vehicle. Trailers can be utility trailers, pop-up campers, travel trailers, livestock trailers, flatbed trailers, enclosed car haulers, and boat trailers, among others. The towing vehicle can be a car, a crossover, a truck, a van, a sports utility vehicle (SUV), a recreational vehicle (RV), or any other vehicle configured to be attached to the trailer and pull the trailer. A trailer hitch can be used to attach the trailer to a powered vehicle. The recipient's hitch is mounted on the towing vehicle and is connected to the trailer hitch to form a connection. The trailer hitch can be a ball and socket joint, a fifth wheel, and a gooseneck or a trailer jack. Other attachment mechanisms can also be used. In addition to the mechanical connection between the trailer and the powered vehicle, in some examples, the trailer is also electrically connected to the towing vehicle. Thus, the electrical connection allows the trailer to take a feed from the power vehicle's rear light circuit, thereby allowing the trailer to have tail lights, turn signals and brake lights that are synchronized with the power vehicle's lights.

[0003] Recent advances in sensor technology have led to improved safety systems for vehicles. Thus, it is desirable to provide a system that can identify in real time the coupler of a trailer behind a towing vehicle and position the coupler so that the towing vehicle can autonomously maneuver toward the trailer for automatic hittingching. Summary of the invention

[0004] One aspect of the present disclosure provides a method for detecting and locating a trailer coupler of a trailer. The method includes: receiving an image at data processing hardware from a camera located on a rear portion of a towing vehicle and in communication with the data processing hardware. The method also includes: determining, by the data processing hardware, a region of interest within the image. The region of interest includes a representation of a trailer coupler. The method also includes: determining, by the data processing hardware, a camera plane in which the camera moves based on the received image. In addition, the method includes: determining, by the data processing hardware, a road plane based on the received image. The method also includes: determining, by the data processing hardware, a three-dimensional (3D) point cloud representing objects within the region of interest and within the camera plane and the road plane. The method includes: receiving sensor data at the data processing hardware from at least one of a wheel encoder, an acceleration and wheel angle sensor, and an inertial measurement unit in communication with the data processing hardware. The method includes: determining, at the data processing hardware, a coupler position of a trailer coupler based on the 3d point cloud and the sensor data. The coupler position is in real world coordinates. Additionally, the method includes sending instructions from the data processing hardware to the steering system to cause the towing vehicle to autonomously steer along a path in a rearward direction toward the coupler location.

[0005] Implementations of the present disclosure may include one or more of the following optional features. In some implementations, determining a region of interest within an image includes: sending instructions from data processing hardware to a display to display a received image; and receiving a user selection of the region of interest at the data processing hardware.

[0006] In some examples, the method further includes: projecting, by data processing hardware, points associated with the 3D point cloud onto a camera plane or a road plane. The method may include: determining, by data processing hardware, a distance between each point and the camera. When the points associated with the 3D point cloud are projected onto the camera plane, the method includes determining a distance between each point and the center of the camera. When the points associated with the 3D point cloud are projected onto the road plane, the method includes determining a distance between each point and a projection of the center of the camera on the road plane. The method may also include: determining, by data processing hardware, a shortest distance based on the determined distances, wherein the projection of the 3D point associated with the shortest distance on the received image represents a connector pixel position within the image. The connector position is based on the connector pixel position.

[0007] In some examples, the method further includes determining, by the data processing hardware, a coupler height based on a distance between the 3D point associated with the shortest distance and the road plane.The coupler position includes the coupler height.

[0008] The method may further include determining, by the data processing hardware, a first distance between the trailer coupler and the camera based on the 3D point cloud; and determining, by the data processing hardware, a second distance between the trailer coupler and the vehicle hitch ball based on the first distance minus a longitudinal distance between the camera and the vehicle hitch ball. The path is based on the second distance.

[0009] In some implementations, determining a point cloud of the region of interest includes executing one of a visual odometry (VO) algorithm, a simultaneous localization and mapping (SLAM) algorithm, and a structure from motion (SfM) algorithm.

[0010] Determining the camera plane may include: determining, by data processing hardware, at least three three-dimensional positions of a rear-facing camera from a received image; and determining, by data processing hardware, the camera plane based on the at least three three-dimensional positions. In some examples, determining the road plane includes: determining a height of the camera from a road supporting the towing vehicle; and shifting the camera plane by the height of the camera.

[0011] In some implementations, determining the road plane includes: extracting, by data processing hardware, at least three feature points including the road from the image; and associating, by the data processing hardware, a point in a 3D point cloud with each feature point. Additionally, determining the road plane may include: determining, by the data processing hardware, the road plane based on at least three points in the 3D point cloud associated with the at least three feature points. In some examples, determining the camera plane includes: determining, by the data processing hardware, a height of the camera from the road; and shifting, by the data processing hardware, the road plane by the height of the camera.

[0012] Another aspect of the present disclosure provides a system for detecting and locating a trailer coupler of a trailer. The system includes: data processing hardware; and memory hardware in communication with the data processing hardware. The memory hardware stores instructions that, when executed on the data processing hardware, cause the data processing hardware to perform operations including the method described above.

[0013] The details of one or more implementations of the disclosure are set forth in the accompanying drawings and the description below. Other aspects, features, and advantages will be apparent from the description and drawings, and from the claims. BRIEF DESCRIPTION OF THE DRAWINGS

[0014] Figure 1 is a schematic top view of an exemplary towing vehicle positioned in front of a trailer.

[0015] Figure 2 yes Figure 1 A schematic diagram of an exemplary tractor vehicle is shown in FIG.

[0016] Figure 3 yes Figure 1 Schematic side view of an exemplary towing vehicle and selected trailer.

[0017] Figure 4A is a perspective view of the towing vehicle and trailer showing the captured image and the area of ​​interest.

[0018] Figure 4B is a perspective view of a semi-dense or dense point cloud for an area of ​​interest within a captured image.

[0019] Figure 5A is a perspective view of a towing vehicle and trailer showing the captured images, the region of interest, and the minimum region of interest.

[0020] Figure 5B is a perspective view of a towing vehicle and trailer showing the captured images, the region of interest, and the minimum region of interest.

[0021] Figure 6 is a schematic diagram of an exemplary arrangement for detecting and locating the operation of a coupler of a trailer hitch associated with a trailer behind a towing vehicle.

[0022] Like reference symbols in the various drawings indicate like elements. DETAILED DESCRIPTION

[0023] refer to Figure 1 and 2A towing vehicle 100, such as, but not limited to, a car, a crossover, a truck, a van, a sport utility vehicle (SUV), and a recreational vehicle (RV), can be configured to hitch to and tow a trailer 200. The towing vehicle 100 is connected to the trailer 200 via a towing vehicle hitch 120 having a vehicle hitch ball 122 connected to a trailer hitch 210 having a trailer coupler 212. It is desirable to have a towing vehicle 100 that can autonomously back up toward a trailer 200 that is identified based on one or more representations 136, 136a-c of the trailer 200, 200a-c displayed on a user interface 130, such as a user display 132. In addition, it is also desirable to have a coupler position estimation and tracking system 160 supported by the towing vehicle 100 that can execute an algorithm that tracks and estimates the position of a coupler 212 associated with the trailer 200 in real time. Thus, the coupler position estimation and tracking system 160 automates the process of hooking up the tractor vehicle 100 to the trailer 200. The coupler position estimation and tracking system 160 may use a single camera 142a and at least one of the following sensors: wheel encoders 144, acceleration and wheel angle sensors 146, and inertial measurement units (IMUs) 148 to determine the position of the coupler 212 in pixel coordinates within the image 143, and the coupler position in a three-dimensional (3D) world.

[0024] refer to Figure 1-5, in some implementations, the driver of the towing vehicle 100 wants to tow the trailer 200 located behind the towing vehicle 100. The towing vehicle 100 can be configured to receive an indication of a driver selection 134 associated with a representation of a selected trailer 200, 200a-c. In some examples, the driver maneuvers the towing vehicle 100 toward the selected trailer 200, 200a-c, while in other examples, the towing vehicle 100 autonomously drives toward the selected trailer 200, 200a-c. The towing vehicle 100 can include a driving system 110 that maneuvers the towing vehicle 100 across the road surface 10 based on a driving command having, for example, x, y, and z components. As shown, the driving system 110 includes a right front wheel 112, 112a, a left front wheel 112, 112b, a right rear wheel 112, 112c, and a left rear wheel 112, 112d. The driving system 110 can also include other wheel configurations. The driving system 110 may also include a braking system 114 including brakes associated with each wheel 112, 112a-d, and an acceleration system 116 configured to adjust the speed and direction of the towing vehicle 100. In addition, the driving system 110 may include a suspension system 118 including tires associated with each wheel 112, 112a-d, tire air, springs, shock absorbers, and linkages connecting the towing vehicle 100 to its wheels 112, 112a-d and allowing relative movement between the towing vehicle 100 and the wheels 112, 112a-d. The suspension system 118 may be configured to adjust the height of the towing vehicle 100, thereby allowing the towing vehicle hitch 120 (e.g., vehicle hitch ball 122) to align with the trailer hitch 210 (e.g., trailer hitch coupler 212), which allows autonomous connection between the towing vehicle 100 and the trailer 200.

[0025] The tractor vehicle 100 can move across the road surface by various combinations of movement about three mutually perpendicular axes defined relative to the tractor vehicle 100: a lateral axis X, a fore-aft axis Y, and a central vertical axis Z. The lateral axis X extends between the right and left sides of the tractor vehicle 100. The forward driving direction along the fore-aft axis Y is designated as F, also referred to as forward motion. Additionally, the rearward or rearward driving direction along the fore-aft direction Y is designated as R, also referred to as rearward motion. When the suspension system 118 adjusts the suspension of the tractor vehicle 100, the tractor vehicle 100 can tilt about the X-axis and / or the Y-axis, or move along the central vertical axis Z.

[0026] The towing vehicle 100 may include a user interface 130. The user interface 130 receives one or more user commands from the driver via one or more input mechanisms or a screen display 132 (e.g., a touch screen display), and / or displays one or more notifications to the driver. The user interface 130 communicates with a vehicle controller 150, which in turn communicates with a sensor system 140. In some examples, the user interface 130 displays an image of the environment of the towing vehicle 100, resulting in the user interface 130 receiving (from the driver) one or more commands to initiate the execution of one or more actions. In some examples, the user display 132 displays one or more representations 136, 136a-c of trailers 200, 200a-c located behind the towing vehicle 100. In this case, the driver selects a representation 136, 136a-c of the trailer 200, 200a-c, thereby causing the controller 150 to execute the coupler position estimation and tracking system 160 associated with the trailer 200, 200a-c of the selected representation 136, 136a-c. In some examples, the controller 150 may automatically, or upon instruction from the driver to autonomously attach to the trailer 200, 200a-c, execute a coupler position estimation and tracking system 160 associated with one trailer 200, 200a-c of the one representation 136, 136a-c, when the user display 132 displays one representation 136, 136a-c of the trailer 200, 200a-c behind the towing vehicle 100. The vehicle controller 150 includes a computing device (or processor or data processing hardware) 152 (e.g., a central processing unit having one or more computing processors) in communication with a non-transitory memory 154 (e.g., a hard disk, flash memory, random access memory, memory hardware) capable of storing instructions executable on the computing processor(s) 152.

[0027] The towing vehicle 100 may include a sensor system 140 to provide reliable and robust driving. The sensor system 140 may include different types of sensors that may be used individually or together to create a sense of the environment of the towing vehicle 100. The sense of the environment is used to help the driver make informed decisions based on objects and obstacles detected by the sensor system 140 or during autonomous driving of the towing vehicle 100. The sensor system 140 may include one or more cameras 142. In some implementations, the towing vehicle 100 includes a rear camera 142a that is mounted to provide an image 143 having a view of the rear driving path of the towing vehicle 100. The rear camera 142a may include a fisheye lens that includes an ultra-wide angle lens that produces strong visual distortion that is intended to create a wide panoramic or hemispherical image 143. The fisheye camera captures the image 143 with an extremely wide angle view. In addition, the image 143 captured by the fisheye camera has a characteristic convex non-straight line appearance. Other types of cameras may also be used to capture the image 143 of the rear driving path of the towing vehicle 100 .

[0028] In some examples, the sensor system 140 also includes one or more wheel encoders 144 associated with one or more wheels 112, 112a-d of the traction vehicle 100. The wheel encoder 144 is an electromechanical device that converts the angular position or movement of the wheel into an analog or digital output signal. Thus, the wheel encoder 144 determines the speed and distance that the wheel 112, 112a-d has traveled.

[0029] The sensor system 140 may also include one or more acceleration and wheel angle sensors 146 associated with the towing vehicle 100. The acceleration and wheel angle sensors 146 determine the acceleration of the towing vehicle 100 in the direction of the lateral X axis and the fore-aft Y axis.

[0030] The sensor system 140 may also include an IMU (Inertial Measurement Unit) 148 that is configured to measure the linear acceleration (using one or more accelerometers) and rotation rate (using one or more gyroscopes) of the towing vehicle. In some examples, the IMU 148 also determines a heading reference of the towing vehicle 100. Thus, the IMU 148 determines the pitch, roll, and yaw of the towing vehicle 100.

[0031] The sensor system 140 may include other sensors such as, but not limited to, radar, sonar, LIDAR (light detection and ranging, which may require optical remote sensing that measures properties of scattered light to find the distance and / or other information of distant objects), LADAR (laser detection and ranging), ultrasonic sensors, stereo cameras, etc. The wheel encoders 144, acceleration and wheel angle sensors 146, IMU 148, and any other sensors output sensor data 145 to the controller 150, i.e., the coupler position estimation and tracking system 160.

[0032] The vehicle controller 150 executes a coupler position estimation and tracking system 160 that receives the image 143 from the rear camera 143a and the sensor data 145 from at least one of the other sensors 144, 146, 148, and based on the received data, the coupler position estimation and tracking system 160 determines the position of the trailer 200, specifically the coupler position L of the coupler 212 associated with the trailer 200. TC For example, trailer 200, 200a-c is identified by a driver via user interface 130. More specifically, coupler position estimation and tracking system 160 determines a pixel position of coupler 212 within received image(s) 143. Additionally, coupler position estimation and tracking system 160 determines a 3D position L of coupler 212 in a three-dimensional (3D) coordinate system or in a global coordinate system. TC In some examples, the coupling position estimation and tracking system 160 also determines the coupling height H of the coupling 212 relative to the road plane 10 in the 3D coordinate system and in the global coordinate system. TC The coupler position estimation and tracking system 160 includes an iterative algorithm that automates the hitching and alignment process for the towing vehicle 100 and trailer 200 .

[0033] The coupling position estimation and tracking system 160 receives the images 143 from the rear camera 142a. For example, because the coupling position estimation and tracking system 160 analyzes all sequences of images 143 received from the camera 142a, rather than just one or two images 143, the coupling position estimation and tracking system 160 is more accurate in making its coupling position L with the coupling 212. TC The determination is more robust.

[0034] In some implementations, the coupling position estimation and tracking system 160 instructs the user interface 130 to display the received image 143 on the display 132 and solicits a selection from the user of a region of interest (ROI) 300 within the displayed image 143 ( Figure 4A and 4B). ROI 300 is a bounding box that includes coupler 212. In other examples, coupler position estimation and tracking system 160 may include a coupler identification algorithm that identifies coupler 212 within image 143 and forms the boundary of coupler 212 by being a bounding box of ROI 300.

[0035] The coupling position estimation and tracking system 160 generates a semi-dense / dense point cloud (eg, coupling 212) of objects within the ROI 300. Figure 4B ). A point cloud is a set of data points in 3D space. More specifically, a point cloud includes multiple points on the outer surface of an object.

[0036] The coupling position estimation and tracking system 160 may use one or more techniques to locate the coupling 212 in the point cloud 400. Some of these techniques include, but are not limited to, visual odometry (VO), simultaneous localization and mapping (SLAM), and structure from motion (SfM). The VO, SLAM, and SfM frameworks are well-known theories and allow the tractor 100 to be positioned in real time in a self-generated 3D point cloud map. VO is a method for determining the position and orientation of the trailer 200, the camera 142a, the coupling 212, or the towing bar 214 by analyzing the image 143 received from the camera 142a. The VO method may extract image feature points and track them in an image sequence. Examples of feature points may include, but are not limited to, edges, corners, or blobs on the trailer 200, the coupling 212, or the towing bar 214. The VO method may also use pixel intensities in an image sequence directly as visual input. The SLAM method constructs or updates a map of an unknown environment while keeping track of one or more targets. In other words, the SLAM method uses the received images 143 as the only source of external information to establish the position and orientation of the towing vehicle 100 and the camera 142a, while simultaneously constructing a representation of the objects in the ROI 300. The SfM method estimates the 3D structure of the objects in the ROI 300 based on the received images 143 (i.e., 2D images). The SfM method can estimate the pose of the camera 142a and the towing vehicle 100 based on the sequence of images 143 captured by the camera 142.

[0037] In some implementations, the coupling position estimation and tracking system 160 is initialized before performing the VO method, the SLAM method, or the SfM method. During the first method of initialization, the coupling position estimation and tracking system 160 sends instructions or commands 190 to the driving system 110, causing the driving system 110 to move the towing vehicle 110 along the front-to-back axis Y in a straight direction (e.g., in the forward driving direction F or the rearward driving direction R) by a predetermined distance. In some examples, the predetermined distance is a few centimeters. The predetermined distance can be between 5 centimeters and 50 centimeters. The forward F and rearward R driving movements along the front-to-back axis Y will cause the SLAM or SfM to be initialized. Additionally, along the forward F and rearward R driving movements, the coupling position estimation and tracking system 160 executes a tracker algorithm to update the ROI 300 within the image 143 provided by the driver or determined by the coupling position estimation and tracking system 160. As the towing vehicle 100 moves in the rearward direction R, the perspective and size of the trailer 200, the drawbar 214, and the coupler 212 change in the image 143. Therefore, the tracker algorithm updates the ROI 300 based on the new images 143 received from the camera 142a during the forward F and rearward R driving movements along the fore-aft axis Y. The ROI 300 includes the coupler 212, and thus, the feature points or pixel intensities in the ROI 300 are tracked by the coupler position estimation and tracking system 160. Since the coupler position estimation and tracking system 160 only analyzes the ROI 300 portion of the image 143, the ROI 300 is used to filter out objects in the image 143 that are not the coupler 212. In some examples, the coupler position estimation and tracking system 160 constructs a visual tracker of the coupler 212 by identifying two-dimensional (2D) feature points in the ROI 300. The coupling position estimation and tracking system 160 then identifies 3D points within the point cloud map that correspond to the identified 2D feature points. Thus, at each iteration of the tracking algorithm (performed by the coupling position estimation and tracking system 160), the coupling position estimation and tracking system 160 projects the selected cloud points 402 onto the 2D camera image 143. The coupling position estimation and tracking system 160 then constructs a minimum ROI 340 that includes the projected 2D points. In this case, the coupling position estimation and tracking system 160 updates the ROI 300 while the towing vehicle is moving, and generates a minimum ROI 340 that includes the previously selected cloud points 402.

[0038] In some implementations, the coupling position estimation and tracking system 160 may be initialized by sending instructions 190 to the driving system 110, causing the driving system 110 to move the towing vehicle 100 toward the center of the ROI 300 by a predetermined distance. In some examples, the predetermined distance is a few centimeters, such as 5 to 50 centimeters. In this case, the coupling position estimation and tracking system 160 updates the ROI 300 during the maneuver of the towing vehicle 100, during the received images 143 or sequence.

[0039] In some implementations, the coupler position estimation and tracking system 160 determines the scale of the 3D point cloud map. When a 3D point cloud map is generated using only a monocular camera, it suffers from scale ambiguity, i.e., a map made using only a monocular camera can only be restored by scale. However, if the coupler position estimation and tracking system 160 does not know the scale of the map, the coupler position estimation and tracking system 160 can determine the scale of the map by fusing the VO, SLAM, or SfM algorithm with the vehicle sensor data 145. In another example, the coupler position estimation and tracking system 160 determines the scale of the map based on the road plane 320 in the 3D point cloud map 400. The coupler position estimation and tracking system 160 determines the distance from the camera position to the road plane 320 in the map 400. The scale of the map 400 is given by the height of the camera 142a (from the camera data 141) divided by the calculated distance between the camera position and the road plane 320 in the map 400. The 3D point cloud map represents the structure of the environment without providing detailed information about the distances of the structures within the map 400. Therefore, the coupler position estimation and tracking system 160 determines the scale of the map 400 including the distance information, and this allows the coupler position estimation and tracking system 160 to determine the position of the coupler 212 in world coordinates.

[0040] The coupling position estimation and tracking system 160 includes a plane determination module 162 configured to determine a camera plane 310 and a road plane 320. In some implementations, the plane determination module 162 determines the camera plane 310 along which the camera 142a moves and the road plane 320. To determine the camera plane 310, the plane determination module 162 uses at least three previous 3D positions of the camera 142a received from the camera 142a as the camera data 141. The camera data 141 may include intrinsic parameters (e.g., focal length, image sensor format, and principal point) and extrinsic parameters (e.g., coordinate system transformation from 3D world coordinates to 3D camera coordinates, in other words, extrinsic parameters define the position of the camera center in the world coordinates and the heading direction of the camera). In addition, the camera data 141 may include the minimum / maximum / average height of the camera 142a relative to the ground (e.g., when the vehicle is loaded and unloaded), and the longitudinal distance between the camera 142a and the vehicle hitch ball 122. The plane determination module 162 determines the camera plane 310 based on the 3D positions of the three points in at least three previous 3D positions of the camera 142a. In some examples, the coupler position estimation and tracking system 160 determines the road plane 320 based on the camera plane 310. In some implementations, since the road plane 320 is a displacement of the camera plane 310 by the height of the camera 142a from the ground (provided in the camera information 141), the plane determination module 162 determines the road plane 320 based on the camera plane 310 and the camera data 141. This process is useful when the three 3D points used to determine the camera plane 310 are co-linear, in which case there are an infinite number of camera planes 310 that are co-planar with the line given by these 3D points.

[0041] In order to determine the road plane 320, the plane determination module 162 extracts at least three feature points associated with the road from the captured 2D image 143. Subsequently, the coupling position estimation and tracking system 160 determines the 3D positions of the three feature points within the point cloud 400, and then the coupling position estimation and tracking system 160 calculates the road plane 320 based on the three feature points. In some examples, the coupling position estimation and tracking system 160 determines the camera plane 310 based on the road plane 320. In some implementations, since the camera plane 310 is a displacement of the road plane 320 by the height of the camera 142a from the ground (provided by the camera information 141), the coupling position estimation and tracking system 160 determines the camera plane 310 based on the road plane 320 and the camera information 141.

[0042] As the towing vehicle 100 moves autonomously in the rearward R direction, the plane determination module 162 may determine and update the planes 310, 320 in real time, or if the plane determination module 162 determines that the road is flat, the coupler position estimation and tracking system 160 may determine the planes 310, 320 only once. The above method uses three points to determine the camera plane 310 or the road plane 320. However, in some examples, the plane determination module 162 may rely on more than three points to determine the planes 310, 320. In this case, the coupler position estimation and tracking system 160 uses a least squares method, a random sampling consensus (RANSAC) method, a support vector machine (SVM) method, or any variation of these algorithms to determine the planes 310, 320. By using more than three points to determine the planes 310, 320, the plane determination module 162 increases robustness to outliers.

[0043] The coupling position estimation and tracking system 160 includes a point cloud point reduction module 164 configured to reduce the size of the ROI 300. In some implementations, the coupling position estimation and tracking system 160 selects 3D cloud points 402 corresponding to the 2D points contained in the ROI 300 in the image 143. The coupling position and estimation system 160 then uses the selected 3D cloud points 402 between the two planes (the road plane 320 and the camera plane 310). The selected 3D cloud points 402 between the two planes 310, 320 are represented as a set M.

[0044] In some examples, the coupler position estimation and tracking system 160 projects the set of points in M ​​(i.e., the selected 3D cloud points 402 between the two planes 310, 320) onto the camera plane 310 or the road plane 320. The projected extracted points are denoted as set J. The coupler position estimation and tracking system 160 then determines the distance from each point in set J to the center of the camera 142a.

[0045] In some implementations, if the first method of initialization is used, the coupling position estimation and tracking system 160 updates the minimum ROI 340 by projecting the points 402 in M ​​onto the current camera 2D image 143. The coupling position estimation and tracking system 160 then determines the updated minimum box 340 that contains the projected points in the camera frame 143 (2D image). The coupling position estimation and tracking system 160 updates the minimum ROI 340 because when time changes or the towing vehicle 100 moves, the 3D point 402 also changes in position relative to the view of the camera 142a. Thus, by projecting the points 402 in the set M onto the image 143, the minimum ROI 340 is updated.

[0046] The coupler position estimation and tracking system 160 includes a coupler detection module 166 configured to detect the coupler 212 and determine a position L of the coupler 212. TC The coupling position estimation and tracking system 160 selects point J' from the set J (i.e., the projected extracted points). Point J' indicates the point from the set J that has the shortest distance between point J' and the camera 142a. As previously mentioned, the set J is the projection of the points 402 in the set M onto the camera plane 310 or the road plane 320. Therefore, when the set J is projected onto the camera plane 310, then point J' is the point closest to the center of the camera (e.g., Figure 3 ). However, if the set J is projected onto the road plane 320, the point J' is a point close to the projection of the camera center onto the road plane 320. In some examples, if J' includes more than one point, the coupler position estimation and tracking system 160 determines the average or median of the points J'. The coupler position estimation and tracking system 160 determines the point associated with J' from the set M and projects the determined point from the set M onto the 2D image 143, which indicates the pixel position of the coupler 212 in the image.

[0047] In some implementations, the coupler detection module 166 determines the coupler position by selecting the N points in a set J that are closest to the camera center (or the projection of the camera center onto the road plane) given a configurable integer parameter N. The set of points is denoted as J*. The coupler detection module 166 determines the mean or median of the set of points J*. The points in the set M associated with J* projected onto the image 143 represent an estimate of the position of the trailer coupler on the image.

[0048] In some implementations, the connector detection module 166 determines the connector location by executing an identification algorithm to find the connector 212 in the point cloud 400. The identification algorithm does not attempt to find the connector in the image 143. The identification algorithm looks for the connector shape in the point cloud (3D world). Another option to simplify this step is to run the identification algorithm in the camera movement plane (or road plane) using the points in the set J.

[0049] The coupler position estimation and tracking system 160 includes a distance estimation module 168 configured to determine a distance D between the trailer coupler 212 and the vehicle hitch ball 122. CC (See Figure 3 ). The distance estimation module 168 determines a first distance D between J′ projected onto the camera moving plane 310 and the camera center (or a projection of J′ projected onto the road plane 320 and the camera center). CJ(as the minimum distance from the camera 142a). The distance estimation module 168 determines the distance D based on the first distance D. CJ Subtract the longitudinal distance D between the camera 142a and the vehicle hitch ball 122 VCC To determine the second distance D between the coupler 212 and the hitch ball 122 CC The second distance indicates the distance D between the trailer coupler 212 and the vehicle tow ball 122. CC .

[0050] The coupling position estimation and tracking system 160 includes a coupling height module 169 that determines the height H of the coupling 212 relative to the road plane 10. TC For example, the coupler height module 169 can determine the coupler position L determined by the coupler detection module 166 TC The distance between the road plane 320 and the coupler. The coupler height module 168 can use the shortest distance between the road plane 320 and the coupler (or use an average point to represent the coupler if the coupler is represented by more than one point in the point cloud) to determine the coupler height H TC .

[0051] Once the coupling position estimation and tracking system 160 determines the coupling height H in the global coordinate system TC and the distance D between the trailer coupler 212 and the vehicle hitch ball 122 CC , the coupler position estimation and tracking system 160 can instruct the path planning system 170 to initiate planning of the path. The controller 150 executes the path planning system 170. The path planning system 170 determines the following path: the path enables the tractor vehicle 100 to autonomously drive in the rear direction R toward the trailer 200 and autonomously connect with the trailer 200.

[0052] As the towing vehicle 100 autonomously maneuvers along the planned path, the path planning system 170 continuously updates the path based on continuously receiving updated information from the coupler position estimation and tracking system 160 and the sensor system 140. In some examples, the object detection system identifies one or more objects along the planned path and sends data related to the location of the one or more objects to the path planning system 170. In this case, the path planning system 170 recalculates the planned path to avoid the one or more objects while also performing the predetermined maneuvers to follow the path. In some examples, the path planning system determines a collision probability, and if the collision probability exceeds a predetermined threshold, the path planning system 170 adjusts the path.

[0053] Once the planned path is determined by the path planning system 170, the vehicle controller 150 executes the driver assistance system 180, which in turn includes a path following behavior 182. The path following behavior 182 receives the planned path and executes one or more behaviors 182a-b that send commands 190 to the driving system 110 to cause the towing vehicle 100 to autonomously drive along the planned path, which causes the towing vehicle 100 to autonomously connect to the trailer 200.

[0054] The path following behaviors 182 include: braking behaviors 182a, speed behaviors 182b, and steering behaviors 182c. In some examples, the path following behaviors 182 also include hitch connection behaviors and suspension adjustment behaviors. Each behavior 182a-182c causes the towing vehicle 100 to take an action, such as driving backward, turning at a specific angle, braking, accelerating, decelerating, among others. The vehicle controller 150 can maneuver the towing vehicle 100 in any direction across the road surface by controlling the driving system 110, more specifically by issuing commands 190 to the driving system 110.

[0055] Braking action 182a may be performed to stop or slow down tractor vehicle 100 based on the planned path. Braking action 182a sends a signal or command 190 to driving system 110 (eg, a braking system (not shown)) to stop or slow down tractor vehicle 100.

[0056] The speed behavior 182b may be executed to change the speed of the towing vehicle 100 by accelerating or decelerating based on the planned path. The speed behavior 182b sends a signal or command 190 to the braking system 114 for deceleration or to the acceleration system 116 for acceleration.

[0057] The steering action 182c may be executed to change the direction of the towing vehicle 100 based on the planned path. Thus, the steering action 182c sends a signal or command 190 indicating a steering angle to the acceleration system 116, thereby causing the driving system 110 to change direction.

[0058] As previously discussed, the coupler position estimation and tracking system 160 determines the position of the trailer coupler 212 and tracks the coupler 212 in real time. In addition, the determined position is based on pixels within the received image 143 and in a global reference system. The coupler position estimation and tracking system 160 uses distance to find the coupler 212 and is configured to filter out cloud points 402 that are not between the camera movement plane 310 and the ground plane 320. Thus, the coupler position estimation and tracking system 160 is feasible for real-time implementation.

[0059] The coupler position estimation and tracking system 160 receives the image 143 from the rear camera 142a, and therefore, the coupler position estimation and tracking system 160 does not require a priori knowledge of the size of the hitch ball 122 or the trailer coupler 212. Additionally, the coupler position estimation and tracking system 160 does not determine the position of the coupler 212 within the image, but instead determines the ROI 300 and then determines the coupler position 212 within the 3D point cloud 400. The coupler position estimation and tracking system 160 uses a standard CPU with or without a GPU or graphics accelerator.

[0060] Figure 6 An exemplary arrangement of operations of a method 600 for using Figure 1 -5 is used to detect and locate the coupler 212 of the trailer hitch 210 associated with the trailer 200 located behind the towing vehicle 100.

[0061] At box 602, the method 600 includes receiving, at the data processing hardware 152, one or more images 143 from a camera 142a located on a rear portion of the towing vehicle 100 and in communication with the data processing hardware 152. At box 604, the method 600 includes determining, by the data processing hardware 152, a region of interest (ROI) 300 within the one or more images 143. The ROI 300 includes a representation of the trailer coupler 212. At box 606, the method 600 includes determining, by the data processing hardware 152, a camera plane 310 in which the camera moves based on the received images 143. At box 608, the method 600 includes determining, by the data processing hardware 152, a road plane 320 based on the received images 143. At box 610, the method 600 includes determining, by the data processing hardware 152, a three-dimensional (3D) point cloud 400 representing objects within the ROI 300 and within the camera plane 310 and the road plane 320. At block 612, the method 600 includes receiving sensor data 145 at the data processing hardware 152 from at least one of the wheel encoder 144, the acceleration and wheel angle sensor 146, and the inertial measurement unit 148 in communication with the data processing hardware 152. At block 614, the method 600 includes determining, at the data processing hardware 152, a coupler position L of the trailer coupler 212 based on the 3D point cloud 400 and the sensor data 145. TC . Connector position L TC In real world coordinates. At block 616, the method 600 includes sending instructions 190 from the data processing hardware 152 to the driving system 110 to cause the towing vehicle 100 to move along a path in the rearward direction R toward the coupling position L. TC Drive autonomously.

[0062] In some implementations, determining the ROI 300 within the image 143 includes: sending instructions from the data processing hardware 152 to the display 132 to display the received image 143 ; and receiving a user selection 134 of the ROI 300 at the data processing hardware 152 .

[0063] The method 600 may also include: projecting, by the data processing hardware 152, the point 402 associated with the 3D point cloud 400 onto the camera plane 310 or the road plane 320. The method 600 may also include: determining, by the data processing hardware 152, the distance between each point and the camera 142a. When the point associated with the 3D point cloud 400 is projected onto the camera plane 310, the method 600 includes determining the distance between each point and the center of the camera 142a. When the point 402 associated with the 3D point cloud 400 is projected onto the road plane 320, the method 600 includes determining the distance between each point and the projection of the center of the camera on the road plane. The method 600 may also include: determining, by the data processing hardware 152, the shortest distance based on the determined distances. The projection of the 3D point associated with the shortest distance from the center of the camera on the received image 143 represents the connector pixel position within the image 143. Connector position L TC In some examples, method 600 includes determining, by data processing hardware 152 , a distance between the 3D point associated with the shortest distance and the road plane 320 , the coupling height H TC . Connector position L TC Including connector height H TC .

[0064] In some implementations, the method 600 includes determining, by the data processing hardware 152 based on the 3D point cloud 400, a first distance D between the trailer coupling 212 and the camera 142a. CJ The method 600 further includes: performing, by the data processing hardware 152, a calculation based on the first distance D CJ Subtract the longitudinal distance D between the camera 142a and the vehicle hitch ball 122 VCC To determine the second distance D between the trailer coupler 212 and the vehicle hitch ball 122 CC The path is based on the second distance D CC .

[0065] In some examples, determining the 3D point cloud 400 of the ROI 300 includes executing one of a visual odometry (VO) algorithm, a simultaneous localization and mapping (SLAM) algorithm, and a structure from motion (SfM) algorithm.

[0066] In some implementations, determining the camera plane 310 includes: determining at least three three-dimensional positions of the rear camera 142a from the received image 143; and determining the camera plane 310 based on the at least three three-dimensional positions. Determining the road plane 320 may include: determining the height of the camera from the road supporting the towing vehicle; and shifting the camera plane 310 toward the road 10 at the height of the camera 142a.

[0067] In some examples, determining the road plane includes: extracting at least three feature points including the road surface 10 from the image 143; associating a point in the 3D point cloud 400 with each feature point; and determining the road plane based on at least three points in the 3D point cloud associated with the at least three feature points. In some examples, determining the camera plane includes: determining the height of the camera from the road; and shifting the road plane by the height of the camera.

[0068] Various implementations of the systems and techniques described herein can be realized in digital electronic circuitry, integrated circuitry, specially designed ASICs (application specific integrated circuits), computer hardware, firmware, software, and / or combinations thereof. These various implementations can include implementations in one or more computer programs that can be executed and / or interpreted on a programmable system that includes at least one programmable processor (which can be special purpose or general purpose) coupled to receive data and instructions from and transmit data and instructions to a storage system, at least one input device, and at least one output device.

[0069] These computer programs (also referred to as programs, software, software applications or code) include machine instructions for a programmable processor and may be implemented in high-level procedural and / or object-oriented programming languages, and / or in assembly / machine languages. As used herein, the terms "machine-readable medium" and "computer-readable medium" refer to any computer program product, apparatus and / or device (e.g., disk, optical disk, memory, programmable logic device (PLD)) for providing machine instructions and / or data to a programmable processor, including a machine-readable medium that receives machine instructions as a machine-readable signal. The term "machine-readable signal" refers to any signal for providing machine instructions and / or data to a programmable processor.

[0070] The functional operations and implementations of the subject matter described in this specification may be implemented in digital electronic circuits, or in computer software, firmware or hardware (including the structures disclosed in this specification and their structural equivalents), or in a combination of one or more of them. In addition, the subject matter described in this specification may be implemented as one or more computer program products, i.e., one or more modules of computer program instructions encoded on a computer storage medium for execution by a data processing device or for controlling the operation of a data processing device. A computer-readable medium may be a machine-readable storage device, a machine-readable storage substrate, a memory device, a material composition that affects a machine-readable propagation signal, or a combination of one or more of them. The terms "data processing device", "computing device" and "computing processor" cover all devices, devices and machines for processing data, including, by way of example, a programmable processor, a computer or multiple processors or computers. In addition to hardware, the device may also include code for creating an execution environment for the computer program in question, such as code constituting processor firmware, a protocol stack, a database management system, an operating system, or a combination of one or more of them. A propagation signal is an artificially generated signal, such as a machine-generated electrical, optical or electromagnetic signal, which is generated to encode information for transmission to a suitable receiver device.

[0071] Similarly, although operations are depicted in a particular order in the accompanying drawings, this should not be understood as requiring such operations to be performed in the particular order shown or in a sequential order, or performing all illustrated operations to achieve the desired result. In some cases, multitasking and parallel processing may be advantageous. In addition, the separation of various system components in the embodiments described above should not be understood as requiring such separation in all embodiments, and it should be understood that the described program components and systems can generally be integrated together in a single software product or encapsulated in multiple software products.

[0072] A number of implementations have been described. Nevertheless, it should be understood that various modifications may be made without departing from the spirit and scope of the present disclosure. Accordingly, other implementations are within the scope of the following claims.

Claims

1. A method for detecting and locating a trailer coupler of a trailer, the method comprising: receiving, at the data processing hardware, an image from a camera located on a rear portion of the towing vehicle and in communication with the data processing hardware; determining, by data processing hardware, a region of interest within the image, the region of interest including a representation of a trailer coupler; determining, by data processing hardware, a camera plane in which the camera is moved based on the received image; determining, by data processing hardware, a road plane based on the received image; determining, by data processing hardware, a three-dimensional (3D) point cloud representing objects within the region of interest and within a camera plane and a road plane; receiving, at data processing hardware, sensor data from at least one of a wheel encoder, an acceleration and wheel angle sensor, and an inertial measurement unit in communication with the data processing hardware; determining, at the data processing hardware, a coupler position of the trailer coupler based on the 3D point cloud and the sensor data, the coupler position in real-world coordinates; as well as sending instructions from the data processing hardware to the steering system to cause the towing vehicle to autonomously steer along a path in a rearward direction toward the coupler position, The method comprises: Projecting points associated with the 3D point cloud onto a camera plane or a road plane by data processing hardware; The data processing hardware determines the distance between each point and the camera: When points associated with the 3D point cloud are projected onto the camera plane, determining the distance between each point and the center of the camera; and When points associated with the 3D point cloud are projected onto the road plane, determining a distance between each point and a projection of the camera center onto the road plane; and A shortest distance is determined by data processing hardware based on the determined distances, a projection of a 3D point associated with the shortest distance onto the received image representing a connector pixel position within the image, wherein the connector position is based on the connector pixel position.

2. The method of claim 1 , wherein determining the region of interest within the image comprises: sending instructions from the data processing hardware to the display to display the received image; as well as A user selection of the region of interest is received at data processing hardware.

3. The method according to claim 1, further comprising: A coupler height is determined by data processing hardware based on a distance between the 3D point associated with the shortest distance and a road plane, wherein the coupler position includes the coupler height.

4. The method according to claim 1, further comprising: determining, by data processing hardware, a first distance between the trailer coupler and the camera based on the 3D point cloud; as well as determining, by the data processing hardware, a second distance between the trailer coupler and the vehicle hitch ball based on the first distance minus a longitudinal distance between the camera and the vehicle hitch ball; Wherein the path is based on a second distance.

5. The method of claim 1 , wherein determining the point cloud of the region of interest comprises: One of a visual odometry (VO) algorithm, a simultaneous localization and mapping (SLAM) algorithm, and a structure from motion (SfM) algorithm is implemented.

6. The method of claim 1 , wherein determining the camera plane comprises: determining, by data processing hardware, at least three three-dimensional positions of the rear camera from the received images; as well as A camera plane is determined by data processing hardware based on the at least three three-dimensional positions.

7. The method of claim 1 , wherein determining a road plane comprises: Determine the height of the camera from the road supporting the towing vehicle; as well as Displaces the camera plane by the camera's height.

8. The method of claim 1 , wherein determining a road plane comprises: extracting at least three feature points including a road from the image by data processing hardware; The data processing hardware associates the points in the 3D point cloud with each feature point; as well as A road plane is determined by data processing hardware based on at least three points in the 3D point cloud associated with the at least three feature points.

9. The method of claim 8, wherein determining the camera plane comprises: determining, by data processing hardware, a height of the camera from the road; as well as The road plane is displaced by the height of the camera by the data processing hardware.

10. A system for detecting and locating a trailer coupler of a trailer, the system comprising: Data processing hardware; as well as Memory hardware in communication with the data processing hardware, the memory hardware storing instructions that, when executed on the data processing hardware, cause the data processing hardware to perform operations comprising: receiving one or more images from a camera located on a rear portion of the towing vehicle and in communication with the data processing hardware; determining a region of interest within the image, the region of interest including a representation of a trailer coupler; determining a camera plane in which the camera moves based on the received image; determining a road plane based on the received image; determining a three-dimensional (3D) point cloud representing objects within the region of interest and within a camera plane and a road plane; receiving sensor data from at least one of a wheel encoder, an acceleration and wheel angle sensor, and an inertial measurement unit in communication with the data processing hardware; determining a coupler position of the trailer coupler based on the 3D point cloud and the sensor data, the coupler position in real world coordinates; and sending a command to the driving system so that the towing vehicle autonomously drives along a path in a rearward direction towards the coupling position, The operations include: Projecting points associated with the 3D point cloud onto the camera plane or the road plane; Determine the distance of each point from the camera: When points associated with the 3D point cloud are projected onto the camera plane, determining the distance between each point and the center of the camera; and When points associated with the 3D point cloud are projected onto the road plane, determining a distance between each point and a projection of the camera center onto the road plane; and A shortest distance is determined based on the determined distances, a projection of a 3D point associated with the shortest distance onto the received image representing a joiner pixel position within the image, wherein the joiner position is based on the joiner pixel position.

11. The system of claim 10, wherein determining the region of interest within the image comprises: Sending instructions to the display so that the received image is displayed; as well as A user selection of the region of interest is received.

12. The system of claim 10, wherein the operations further comprise: A coupler height is determined based on a distance between the 3D point associated with the shortest distance and a road plane, wherein the coupler position includes a coupler height.

13. The system of claim 10, wherein the operations further comprise: determining a first distance between the trailer coupler and the camera based on the 3D point cloud; as well as determining a second distance between the trailer coupler and the vehicle hitch ball based on the first distance minus a longitudinal distance between the camera and the vehicle hitch ball; Wherein the path is based on a second distance.

14. The system of claim 10, wherein determining the 3D point cloud of the region of interest comprises: One of a visual odometry (VO) algorithm, a simultaneous localization and mapping (SLAM) algorithm, and a structure from motion (SfM) algorithm is implemented.

15. The system of claim 10, wherein determining the camera plane comprises: determining, by data processing hardware, at least three three-dimensional positions of the rear camera from the received images; as well as A camera plane is determined by data processing hardware based on the at least three three-dimensional positions.

16. The system of claim 10, wherein determining a road plane comprises: Determine the height of the camera from the road supporting the towing vehicle; as well as Displaces the camera plane by the camera's height.

17. The system of claim 10, wherein determining a road plane comprises: extracting at least three feature points including a road from the image; Associate points in the 3D point cloud with each feature point; as well as A road plane is determined based on at least three points in the 3D point cloud associated with the at least three feature points.

18. The system of claim 17, wherein determining the camera plane comprises: determining the height of the camera from the road; as well as Displaces the road plane by the camera's height.

Images