Coupler and drawbar detection for automatic trailer hitch via cloud point

Abstract

A method for detecting and locating a coupler (212) of a trailer hitch (210) associated with a trailer (200) located behind a towing vehicle (100) is provided. The method includes determining a region of interest (300) within an image (143) from a rear camera (142). The region of interest (300) includes a representation of the trailer hitch (210). The method includes determining a three-dimensional (3D) point cloud (400) representing objects within the region of interest. The method includes determining a camera plane (310) based on the image. The method includes projecting the cloud points (402) onto the camera plane (310); and determining a coupler position based on the cloud points (402) projected onto the camera plane (310). The method includes determining a path based on the coupler position. The path ends when the towing vehicle is attached to the trailer. The method also includes sending instructions to a driving system (110) to cause the towing vehicle to autonomously drive in a rearward direction along the path.

Description

Technical Field

[0001] The present disclosure relates to a method and apparatus for detecting a coupler and a tow-bar of a trailer during automatic trailer hitching via a three-dimensional point cloud map. 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 the coupler and drawbar of a trailer behind a towing vehicle in real time and enable real time coupler and drawbar positioning, thereby allowing the towing vehicle to autonomously maneuver toward the trailer for automatic hitching. Summary of the invention

[0004] One aspect of the present disclosure provides a method for detecting and locating a coupler of a trailer hitch associated with a trailer located behind a towing vehicle. The method includes receiving an image at data processing hardware from a rear camera located on a rear portion of the 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 the trailer hitch. The method also includes determining, by the data processing hardware, a three-dimensional (3D) point cloud representing objects within the region of interest. The method also includes determining, by the data processing hardware, a camera plane in which the rear camera moves based on the received image. The method also includes projecting, by the data processing hardware, the cloud points onto the camera plane. The method includes determining, by the data processing hardware, a coupler position based on the cloud points projected onto the camera plane. The method also includes determining, by the data processing hardware, a path based on the coupler position. The path ends when the towing vehicle is attached to the trailer. The method also includes sending instructions from the data processing hardware to a driving system to autonomously drive the towing vehicle along the path in a rearward direction.

[0005] Implementations of the present disclosure may include one or more of the following optional features. In some implementations, the method further includes: determining, by data processing hardware, an alignment direction of the trailer relative to the vehicle based on the cloud points projected onto the camera plane. The path may also be based on the alignment direction.

[0006] In some examples, the method includes: determining, by data processing hardware, a longitudinal distance between the rear camera and the coupler. The method may include: determining a plane perpendicular to the alignment direction; and determining a first distance between the rear camera and the plane. The first distance indicates a longitudinal distance. The method may also include: projecting the position of the rear camera on the plane; and determining a second distance between a projection of the position of the rear camera on the plane and a projection of the coupler in the camera plane. The second distance indicates a longitudinal distance. The path may also be based on the longitudinal distance and / or the lateral distance.

[0007] In some implementations, determining a region of interest within the image includes: sending instructions from data processing hardware to a display to display the received image; and receiving a user selection of the region of interest at the data processing hardware.

[0008] Determining the point cloud of the area of ​​interest may include executing one of a visual odometry (VO) algorithm, a simultaneous localization and mapping (SLAM) algorithm, and a structure from motion (SfM) algorithm. In some examples, the method includes initializing data processing hardware prior to executing the algorithms by sending a command from the data processing hardware to the driving system to drive the towing vehicle in a forward or backward straight direction for a predetermined distance.

[0009] In some implementations, determining the camera plane includes: determining, by data processing hardware, at least three three-dimensional positions of the rear camera from the received image. Additionally, determining the camera plane may also include: determining, by data processing hardware, the camera plane based on the three three-dimensional positions.

[0010] In some examples, the method further includes determining, by the data processing hardware, a road plane by: determining a height of the rear camera from the road supporting the towing vehicle; and shifting the camera plane by the height of the rear camera.

[0011] In some examples, the method includes determining a road plane by: 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 point cloud with each feature point. The method for determining a road plane may also include: determining, by the data processing hardware, the road plane based on at least three points in the 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 rear camera from the road; and shifting, by the data processing hardware, the road plane by the height of the rear camera.

[0012] In some implementations, the method further includes determining, by the data processing hardware, a height of the coupler based on a distance between the coupler and a plane of the road.The path may also be based on a height of the coupler.

[0013] Another aspect of the present disclosure provides a system for detecting and locating a coupler of a trailer hitch associated with a trailer located behind a towing vehicle. 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.

[0014] 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

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

[0016] Figure 2 is a schematic diagram of an exemplary towing vehicle with a user interface, a sensor system, and a controller.

[0017] Figure 3 is a perspective view of a towing vehicle and trailer.

[0018] Figure 4 It is a perspective view of a semi-dense or dense point cloud for a region of interest ROI within the captured image.

[0019] Figure 5 is a perspective side view of the projection of the point cloud onto the camera plane in which the camera moves.

[0020] Fig. 6A and 6B is a schematic top view of the projection of the point cloud into the camera plane.

[0021] Figure 7 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 2, a 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), may 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 towing 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. Additionally, it is desirable to have a position estimation and tracking system 160 supported by the towing vehicle 100 that is capable of executing an algorithm for real-time tracking and estimating the position of the trailer hitch 210 (i.e., the coupler 212 and the drawbar 214 of the trailer 200). The position estimation and tracking system 160 may capture images 143 using a single camera 142a located on the rear portion of the towing vehicle 100, and analyze the images 143 to identify and track the coupler 212 and the drawbar 214 of the trailer 200. The position estimation and tracking system 160 generates a semi-dense / dense point cloud 400 (e.g., a region of interest (ROI) 300) within each captured image 143. Figure 4 and 5 ), and based on the point cloud 400, the position estimation and tracking system 160 locates the coupler 212 and the tow bar 214 within the point cloud 400. The positioning of the coupler 212 and the tow bar 214 in the point cloud 400 may be generated by well-known frameworks such as, but not limited to, visual odometry (VO), simultaneous localization and mapping (SLAM), and structure from motion (SfM). Additionally, the position estimation and tracking system 160 determines the camera pose of the rear camera 142a within the generated point cloud 400. Based on the determined camera pose, the position estimation and tracking system 160 determines the camera plane 310 ( Figure 3 ). Position estimation and tracking system 160 can then use camera plane 310 to reduce the complexity of the search space for coupler 212 and tow bar 214 within received image 143. When position estimation and tracking system 160 identifies coupler 212 and the coupler position within image 143, position estimation and tracking system 160 uses the shape of tow bar 214 to find alignment direction 220 of trailer 200.

[0024] refer to Figure 1-6, 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 a road surface 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 that connect the towing vehicle 100 to its wheels 112, 112a-d and allow 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., the towing vehicle hitch ball 122) to align with the trailer hitch 210 (e.g., the trailer hitch coupler 212), which allows an 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 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 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, which is used by the towing vehicle 100 to drive and help the driver make informed decisions based on objects and obstacles detected by the sensor system 140. 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 motion of the wheel into an analog or digital output signal. Thus, the wheel encoder 144 determines the speed and distance the wheel 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, which is configured to measure the linear acceleration (using one or more accelerometers) and rotation rate (using one or more gyroscopes) of the vehicle. In some examples, the IMU 148 also determines the 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, and IMU 148 output sensor data 145 to the controller 150, i.e., the position estimation and tracking system 160.

[0032] The vehicle controller 150 executes a position estimation and tracking system 160 that receives the image 143 from the rear camera 142a and determines the position of the trailer 200 relative to the towing vehicle 100 identified by the driver via the user interface 130. More specifically, the position estimation and tracking system 160 determines the position of the coupler 212 and the position of the tow bar 214 within the received image 143. In addition, the position estimation and tracking system 160 determines the position of the coupler 212 and the position of the tow bar 214 in a three-dimensional (3D) coordinate system or a global coordinate system. In some examples, the position estimation and tracking system 160 also determines an alignment direction 220 of the towing vehicle 100 to align with the trailer 200 so that the front-rear axis Y of the towing vehicle 100 overlaps the front-rear axis Y of the trailer 200. The position estimation and tracking system 160 includes an iterative algorithm that enables the vehicle-trailer system (see Figure 3 ) to automate the mounting and alignment process.

[0033] The position estimation and tracking system 160 receives the image 143 from the rear camera 142a. In some implementations, the 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. The ROI 300 is a bounding box that includes the trailer hitch 210. In other examples, the position estimation and tracking system 160 may include a drawbar-coupler identification algorithm that identifies the trailer hitch 210 within the image 143 and forms the boundaries of the trailer hitch 210 by being the bounding box of the ROI 300.

[0034] The position estimation and tracking system 160 generates a semi-dense / dense point cloud 400 of the tow bar 214 and the coupler 212, as well as a portion of the trailer 200 within the ROI 300, such as Figure 4 As shown in . A point cloud is a set of data points in space, and more specifically, a point cloud includes multiple points on the outer surface of an object.

[0035] The position estimation and tracking system 160 may use one or more techniques to locate the tow bar 214 and the coupler 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). VO is a method for determining the position and orientation of the trailer 200, the camera 142a, the coupler 212, or the tow bar 214 by analyzing the images 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 coupler 212, or the tow bar 214. The VO method may also directly use pixel intensities in the image sequence 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 constructing a representation of the objects in the ROI 300. The SfM method estimates the 3D structure of the object in the ROI 300 based on the received image 143 (ie, 2D image). The SfM method may estimate the pose of the camera 142a and the towing vehicle 100 based on the sequence of images 143 captured by the camera 142.

[0036] In some implementations, the position estimation and tracking system 160 performs a SLAM method or a SfM method, and thus the position estimation and tracking system 160 is first initialized. Thus, the position estimation and tracking system 160 sends an instruction or command 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 a forward driving direction F or a rearward driving direction R) by a predetermined distance. In some examples, the predetermined distance is a few centimeters. The predetermined distance may be between 5 cm and 50 cm. The forward F and rearward R driving movements along the front-to-back axis Y cause the SLAM or SfM to be initialized. Additionally, along the forward F and rearward R driving movements, the 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 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 tow bar 214, and the coupler 212 change in the image 143. The ROI 300 includes the tow bar and the coupler so that feature points or pixel intensities in the ROI 300 are tracked by the position estimation and tracking system 160. The ROI 300 is used to filter out anything in the image 143 that is not the tow bar 214 and the coupler 212.

[0037] The position estimation and tracking system 160 analyzes all sequences of received images 143 instead of two images. Therefore, the position estimation and tracking system 160 is more robust in making its determination. The VO, SLAM and SfM frameworks, which are recognized theories, allow the vehicle to be localized in real time in a self-generated 3D point cloud map. The 3D point cloud map generated by the VO, SLAM or SfM algorithm allows the plane determination module 162 to find the trailer coupler 212 in the 3D world (not in the image frame).

[0038] In some implementations, the position estimation and tracking system 160 determines the scale of the 3D point cloud map based on one of the VO, SLAM or SfM algorithms. When a 3D point cloud map is generated by 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 position estimation and tracking system 160 does not know the scale of the map, the 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 position estimation and tracking system 160 determines the scale of the map based on the road plane 320. The position estimation and tracking system 160 determines the distance from the camera position to the road plane 320 in the map. The scale of the map is given by the height of the camera 142a (from the camera data 141) divided by the distance calculated in this step. The 3D point cloud map represents the structure of the environment without providing detailed information about the distance of the structure within the map. Thus, the position estimation and tracking system 160 determines the scale of the map including the distance information, and this allows the position estimation and tracking system 160 to determine the position of the coupler 212 in world coordinates.

[0039] The position estimation and tracking system 160 includes a plane determination module 162, which is 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 and the road plane 320 along which the camera 142a moves. 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 towing 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 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.

[0040] 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 position estimation and tracking system 160 determines the 3D positions of the three feature points within the point cloud 400, and then the position estimation and tracking system 160 calculates the road plane 320 based on the three feature points. In some examples, the 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 position estimation and tracking system 160 determines the camera plane 310 based on the road plane 320 and the camera information 141.

[0041] 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 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 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.

[0042] Once the camera plane 310 and the road plane 320 are determined by the plane determination module 162, the detection module 164, which is part of the position estimation and tracking system 160, determines the position of the tow bar 214 and the coupler 212. Figure 5 As shown in , the detection module 164 projects the cloud points 402 corresponding to the ROI 300 onto the camera plane 310. The detection module 164 then executes a computer vision-based identification algorithm (e.g., a neural network, a correlation filter, etc.) to find the known shape of the traction rod 214 or the coupling 212 as projected on the camera plane 310. In some examples, the characteristics of the shapes of the traction rod 214 and the coupling 212 may be stored in the memory hardware 154, or may be trained to identify the shapes using a training algorithm. The detection module 164 may then identify the known shape of the coupling 212 based on the determined position L TC and the determined position L of the traction rod 214 TB , and the shape of the trailer hitch 210 determine the alignment direction 220 of the trailer 200 relative to the towing vehicle 100 ( Fig. 6A and Figure 6B ). Thus, by projecting point 402 onto camera plane 310, detection module 164 determines the position of coupler 212 in camera plane 310, and based on the position of coupler 212 in camera plane 310, detection module 164 determines the position of coupler 212 in real-world 3D coordinates. Additionally, by projecting point 402 onto camera plane 310, detection module 164 determines the alignment direction of trailer 200 relative to towing vehicle 100 within camera plane 310. Detection module 164 determines the position of both tow bar 214 and coupler 212, which allows vehicle 100 to be aligned with trailer 200.

[0043] In some implementations, the distance estimation module 166 determines the coupling position L in the camera plane 310 by TC The camera position L received from the camera data 141 VC The distance between the connector 212 and the rear camera 142a is estimated by using the distance between the connector 212 and the rear camera 142a. Fig. 6A and 6B , the distance estimation module 166 can determine the plane F formed by the alignment direction 220 (which is the normal to the plane F) and the position of the coupling 212 in the camera plane 310. The longitudinal distance D Lon is the distance between the position of camera 142a and plane F. The lateral distance D Lat is the camera position L VC The distance between the projection 142a ′ onto F and the projection 212 ′ of the coupler position in the camera plane 310 .

[0044] In some implementations, the coupler height module 168 determines the height of the coupler 212 relative to the ground (not shown). For example, the coupler height module 168 can determine the coupler position L determined by the detection module 164. TC The distance from the road plane 320. The coupler height module 168 can use the shortest distance between the road plane 320 and the coupler point (if the coupler is represented by more than one point in the point cloud, an average point is used to represent the coupler).

[0045] Position and estimation tracking system 160 detects traction rod 214 and coupler 212 and then determines the coupler position L TC and its height from the ground, and the position L of the camera 142a and the connector TC The distance between (i.e., the longitudinal distance D Lon and lateral distance D Lat ). Based on the information determined by the position and estimation tracking system 160, the controller 150 executes the path planning system 170. The path planning system 170 determines a path that enables the towing vehicle 100 to autonomously drive in the rearward direction R toward the trailer 200 and autonomously connect with the trailer 200.

[0046] 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 position estimation and tracking system 160. 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. 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 and sends it to the driver assistance system 180.

[0047] 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, thereby causing the vehicle 100 to autonomously drive along the planned path, which causes the towing vehicle 100 to autonomously connect to the trailer 200.

[0048] 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.

[0049] Braking action 182a may be performed to stop or slow 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 towing vehicle 100.

[0050] 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.

[0051] 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.

[0052] As previously discussed, the position estimation and tracking system 160 determines the position L of the coupling by projecting the cloud point 402 onto the camera plane 310. TC Additionally, the position estimation and tracking system 160 filters outliers by calculating the camera movement plane 310 and the road plane 320. For example, an outlier is a point that may have been found on the ground. Only points above the ground and below the camera plane 310 are considered.

[0053] In some implementations, the position estimation and tracking system 160 generates a semi-dense or dense point cloud of the captured image 143, rather than just the ROI 300 as previously described. By generating a semi-dense or dense point cloud of the captured image 143, the position estimation and tracking system 160 can detect objects within the image 143, and thus, the path planning system 170 can use the identified objects to adjust the planned path and avoid the detected objects.

[0054] Position estimation and tracking system 160 receives images from a single rear-facing camera 142a, and therefore, does not require a priori knowledge of the size of trailer coupler 212 in image 143. Position estimation and tracking system 160 uses a standard CPU with or without a GPU or graphics accelerator.

[0055] Figure 7 An exemplary arrangement of operations of a method 700 for using Figure 1-6B100 to detect and locate a coupler 212 of a trailer hitch 210 associated with a trailer 200 located behind a towing vehicle 100. At box 702, the method 700 includes receiving, at the data processing hardware 152, an image 143 from a rear camera 142a located on a rear portion of the towing vehicle 100 and in communication with the data processing hardware 152. In some examples, the data processing hardware 152 communicates with memory hardware 154, which is configured to store instructions, images 143, and / or data. At box 704, the method 700 includes determining, by the data processing hardware 152, a region of interest 300 within the image 143. The region of interest 300 includes a representation of the trailer hitch 210. At box 706, the method 700 includes determining, by the data processing hardware 152, a three-dimensional (3D) point cloud 400 representing objects within the region of interest 300. At block 708, the method 700 includes determining, by the data processing hardware 152, a camera plane 310 in which the rear camera 142a moves based on the received image 143. At block 710, the method 700 includes projecting, by the data processing hardware 152, cloud points 402 associated with the point cloud 400 onto the camera plane 310. At block 712, the method 700 includes determining, by the data processing hardware 152, a coupling position L based on the cloud points 402 projected onto the camera plane 310. TC At block 714, method 700 includes: performing, by data processing hardware 152, a calculation based on the coupling position L TC The path begins at the current position of the tractor vehicle 100 and ends when the tractor vehicle 100 is attached to the trailer 200. At box 716, the method 700 includes sending instructions 190 from the data processing hardware 152 to the driving system 110 to cause the tractor vehicle 100 to autonomously drive in the rear direction R along the path.

[0056] In some implementations, the method 700 further includes: determining, by the data processing hardware 152, an alignment direction 220 of the trailer 200 relative to the towing vehicle 100 based on the cloud points 402 projected onto the camera plane 310. The path can also be based on the alignment direction 220. In some examples, the method 700 includes: determining, by the data processing hardware 152, a longitudinal distance and a lateral distance between the rear camera and the coupler 212. The method includes: determining a plane F perpendicular to the alignment direction 220; determining a first distance d between the rear camera 142a and the plane F; Lon The first distance d Lon Indicates the longitudinal distance d LonThe method 700 may further include: projecting the position of the rear camera 142a onto the plane F as a camera projection 142a'; and determining a second distance D between the projection 142a' of the rear camera 142a' onto the plane F and the projection 212' of the coupler 212 onto the camera plane 310. Lat The second distance d Lat Indicates the longitudinal distance d Lat The path can also be based on the longitudinal distance D Lon and lateral distance D Lat .

[0057] In some examples, determining the region of interest 300 within the image 143 (at block 704 ) includes sending instructions from the data processing hardware 152 to a display to display the received image 143 ; and receiving a user selection of the region of interest 300 at the data processing hardware 152 .

[0058] In some implementations, determining the point cloud 400 of the area of ​​interest 300 (at block 706) includes executing one of a visual odometry (VO) algorithm, a simultaneous localization and mapping (SLAM) algorithm, and a structure from motion (SfM) algorithm. In some examples, the method 700 also includes initializing the data processing hardware 152 before executing the algorithms by sending instructions from the data processing hardware 152 to the driving system 110 to drive the towing vehicle 110 in a straight direction in the forward direction F or the rearward direction R for a predetermined distance.

[0059] In some examples, determining the camera plane 310 (at block 708) includes: determining, by the data processing hardware 152, at least three-dimensional positions of the rear camera from the received image 143; and determining, by the data processing hardware 152, the camera plane 310 based on the three-dimensional positions. In some implementations, the method 700 also includes: determining, by the data processing hardware 152, the road plane 320 by: determining the height of the rear camera 142a from the road supporting the towing vehicle 100; and shifting the camera plane 310 by the height of the rear camera.

[0060] In some implementations, method 700 includes determining road plane 320 by: extracting, by data processing hardware 152, at least three feature points including the road from image 143; associating, by data processing hardware 152, point 402 in point cloud 400 with each feature point; and determining, by data processing hardware 152, road plane 320 based on at least three points in point cloud 400 associated with the at least three feature points. In some examples, determining camera plane 310 includes: determining, by data processing hardware 152, a height of rear camera 142a from the road; and shifting, by data processing hardware 152, road plane 320 by the height of rear camera 142a.

[0061] The method 700 includes determining, by the data processing hardware 152 , a height of the coupler 212 based on a distance between the coupler 212 and the road plane 320 , wherein the path is based on the height of the coupler 212 .

[0062] 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.

[0063] 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.

[0064] 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.

[0065] 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.

[0066] 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 coupler of a trailer hitch associated with a trailer located behind a towing vehicle, the method comprising: receiving, at the data processing hardware, an image from a rear-facing camera located on a rear portion of the 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 hitch; determining, by data processing hardware, a three-dimensional (3D) point cloud representing objects within the region of interest; determining, by data processing hardware, a camera plane in which the rear camera moves based on the received image; The cloud points are projected onto the camera plane by the data processing hardware; Determining the coupling position by data processing hardware based on the cloud points projected onto the camera plane; determining, by data processing hardware, a path based on the coupler position, the path ending when the towing vehicle is attached to the trailer; as well as sending instructions from the data processing hardware to the driving system so that the towing vehicle is driven autonomously in a rearward direction along the path, Wherein the method further comprises: An alignment of the trailer relative to the vehicle is determined by data processing hardware based on the cloud points projected onto the camera plane, wherein the path is based on the alignment.

2. The method according to claim 1, further comprising: The data processing hardware determines the longitudinal and lateral distances between the rear camera and the connector in the following manner: determining a plane perpendicular to the alignment direction; determining a first distance between the rear camera and the plane, the first distance indicating a longitudinal distance; Projecting the position of the rear camera onto the plane; as well as determining a second distance between a projection of the position of the rear camera on the plane and a projection of the coupler in the camera plane, the second distance being indicative of a longitudinal distance; The path is based on a longitudinal distance and a lateral distance.

3. 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.

4. 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.

5. The method of claim 4, further comprising initializing data processing hardware prior to executing the algorithm by: Instructions are sent from the data processing hardware to the steering system to cause the towing vehicle to be driven in a straight direction, either forward or reverse, for a predetermined distance.

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 three three-dimensional positions.

7. The method according to claim 6, further comprising: The road surface is determined by the data processing hardware in the following way: Determining the height of the rear camera from the road supporting the towing vehicle; and Displaces the camera plane by the height of the rear camera.

8. The method of claim 1 further comprising determining the road plane by: extracting at least three feature points including a road from the image by data processing hardware; Associating, by data processing hardware, points in the point cloud with each feature point; and A road plane is determined by data processing hardware based on at least three points in the 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 rear camera from the road; as well as The road plane is displaced by the height of the rear camera by the data processing hardware.

10. The method according to claim 1, further comprising: determining, by data processing hardware, a height of the coupler based on a distance between the coupler and a road plane; The path is based on the height of the coupler.

11. A system for detecting and locating a coupler of a trailer hitch associated with a trailer positioned behind a towing vehicle, 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 an image from a rear camera located on a rear portion of the 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 hitch; determining a three-dimensional (3D) point cloud representing objects within the region of interest; determining a camera plane in which the rear camera moves based on the received image; Project the cloud points onto the camera plane; Determining a connector position based on the cloud points projected onto the camera plane; determining a path based on the coupler position, the path ending when the towing vehicle is attached to the trailer; and sending instructions to a driving system so that the towing vehicle autonomously drives in a rearward direction along the path, wherein the operations further include: determining an alignment direction of the trailer relative to the vehicle based on the cloud points projected onto the camera plane; Wherein the path is based on the alignment direction.

12. The system of claim 11, wherein the operations further comprise determining a longitudinal distance and a lateral distance between the rear camera and the coupler by: determining a plane perpendicular to the alignment direction; determining a first distance between the rear camera and the plane, the first distance indicating a longitudinal distance; Projecting the position of the rear camera onto the plane; as well as determining a second distance between a projection of the position of the rear camera on the plane and a projection of the coupler in the camera plane, the second distance being indicative of a longitudinal distance; The path is based on a longitudinal distance and a lateral distance.

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

14. The system of claim 11, 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.

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

16. The system of claim 15, wherein the operations further comprise: The road surface is determined as follows: Determining the height of the rear camera from the road supporting the towing vehicle; and displacing the camera plane by the height of the rear camera; and 17. The system of claim 11, wherein the operations further comprise: The road surface is determined as follows: extracting at least three feature points including a road from the image; Associating points in the point cloud with each feature point; and A road plane is determined based on at least three points in the point cloud associated with the at least three feature points.

18. The system of claim 11, wherein the operations further comprise: determining a height of the coupling based on a distance between the coupling and a road plane; The path is based on the height of the coupler.

Images