Patent Description:
Trailers are usually unpowered vehicles that are pulled by a powered tow vehicle. A trailer may be a utility trailer, a popup camper, a travel trailer, livestock trailer, flatbed trailer, enclosed car hauler, and boat trailer, among others. The tow vehicle may be a car, a crossover, a truck, a van, a sports-utility-vehicle (SUV), a recreational vehicle (RV), or any other vehicle configured to attach to the trailer and pull the trailer. The trailer may be attached to a powered vehicle using a trailer hitch. A receiver hitch mounts on the tow vehicle and connects to the trailer hitch to form a connection. The trailer hitch may be a ball and socket, a fifth wheel and gooseneck, or a trailer jack. Other attachment mechanisms may also be used. In addition to the mechanical connection between the trailer and the powered vehicle, in some examples, the trailer is electrically connected to the tow vehicle. As such, the electrical connection allows the trailer to take the feed from the powered vehicle's rear light circuit, allowing the trailer to have taillights, turn signals, and brake lights that are in sync with the lights of the powered vehicle.

<CIT> discloses a system and method to assist in coupling a vehicle to a trailer, wherein an inertial measurement unit is provided for detecting motion of the vehicle. A camera positioned on a rear portion of the vehicle provides an image of a region to a rear of the vehicle, wherein a position of a trailer coupler of the trailer is determined in the image, and wherein a spatial location of the trailer coupler is calculated from the determined position. Then a path between an identified spatial location of a hitch ball of the vehicle and the spatial location of the trailer coupler is calculated. In order to identify the spatial location of the hitch ball a position of the hitch ball is determined in the image based on a pattern matching routine, such as a circular Hough Transform. Determining the position of the trailer coupler employs that a presumed height of the trailer coupler relative to the ground is known and stored in a memory in order to narrow the region of the image in which the trailer coupler may be located, wherein determining the position of the trailer coupler my alternatively utilize multiple images form the camera and a motion stereo approach.

<CIT> discloses an automatic control system that moves a tow vehicle so that it can be coupled to a trailer, wherein a visual target that has a chessboard pattern is mounted on the trailer, and wherein one or more cameras positioned on a rear portion of the tow vehicle generate images of the target. A motion control unit of the tow vehicle comprising a perception engine and a controller processes said images and generates tow vehicle movement command signals as a function of the images. Thereby, the perception engine includes a process of searching for the target within a determined region of interest (ROI) in a received image. This process creates a sub-image based on the ROI, converts the sub-image to black-and-white, find the pixel coordinates of the corners of the target pattern, refines the pixel coordinates of the corners and estimate extrinsic camera parameters (translation and rotation matrices). Then, the process updates the ROI and directs back to image capture step, wherein in order to speed up image processing only the ROI which only includes the view of the target is processed. The image which contains the target is analyzed and the tow vehicle position information is generated, and the position and orientation of the target is calculated from the differences between the current translation and rotation and the initial translation and rotation. The document <CIT> discloses a vision based image system.

Recent advancements in sensor technology have led to improved safety systems for vehicles. As such, it is desirable to provide a system that is capable of determining a position of a trailer relative to a tow vehicle, which allows the tow vehicle to autonomously maneuver towards the trailer and autonomously hitch with the trailer.

One aspect of the disclosure provides a method for autonomously maneuvering a tow vehicle towards a trailer for autonomous hitching between the trailer and the tow vehicle. The method includes receiving, at data processing hardware, images from one or more cameras positioned on a back portion of the tow vehicle and in communication with the data processing hardware. The method also includes receiving, at the data processing hardware, sensor data from an inertial measurement unit in communication with the data processing hardware and supported by the tow vehicle. The method also includes determining, at the data processing hardware, a pixel-wise intensity difference between a current received image and a previous received image. The method includes determining, at the data processing hardware, a camera pose and a trailer pose with respect to a world coordinate system. The camera pose and the trailer pose are based on the images, the sensor data, and the pixel-wise intensity difference. The method also includes determining, at the data processing hardware, a tow vehicle path based on the camera pose and the trailer pose. The method also includes sending instruction from the data processing hardware to a drive system supported by the tow vehicle. The instructions causing the tow vehicle to autonomously maneuver along the tow vehicle path in a reverse direction so that the tow vehicle hitches with the trailer.

Implementations of the disclosure may include one or more of the following optional features. In some implementations, the one or more cameras include a fisheye lens that includes an ultra wide-angle lens. The camera pose may include a camera location and a camera angle in the world coordinate system. The trailer pose may include a trailer location and a trailer angle in the world coordinate system.

In some examples, the method further includes identifying one or more feature points within the images. The feature points are associated with the trailer. The method may also include determining a plane based on the one or more feature points. The plane is indicative of a front face of the trailer. The method may also include determining a normal line to the plane. In some examples, the method also includes determining the trailer angle based on the normal line.

In some implementations, the method may include determining a region of interest surrounding a representation of the trailer within the images. Determining a pixel-wise intensity difference between the current received image and a previous received image, includes determining the pixel-wise intensity difference between the region of interest of the current received image and the region of interest of the previous received image. The method may further include identifying one or more feature points within the region of interest. The feature points are associated with the trailer. The method may also include tracking the one or more feature points as the vehicle autonomously maneuvers along the path.

In some examples, the method further includes determining a sequence of points associated with the path. The tow vehicle follows the sequence of points. The method may also include determining a steering wheel angle associated with each point within the sequence of points. The method may also include sending instructions to the drive system to adjust a current steering wheel angle based on the determined steering wheel angle. In some examples, the method further includes determining a difference between a steering angle of each front wheel of the tow vehicle, as the vehicle autonomously maneuver along the path. The method may also include sending instructions to the drive system to adjust the steering wheel angle of each wheel based on the difference between the steering wheel angle of each front wheel.

Another aspect of the disclosure provides a system for autonomously maneuvering a tow vehicle towards a trailer for autonomous hitching between the trailer and the tow 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 that include the method described above.

A tow vehicle, such as, but not limited to a car, a crossover, a truck, a van, a sports-utility-vehicle (SUV), and a recreational vehicle (RV) may be configured to tow a trailer. The tow vehicle connects to the trailer by way of a trailer hitch. It is desirable to have a tow vehicle that is capable of autonomously backing up towards a trailer identified from one or more trailer representations of trailers displayed on a user interface, such as a user display. In addition, it is also desirable to have a tow vehicle that is capable of estimating a trailer position, for example, a trailer angle and position relative to a tow vehicle, based on one or more images received from a camera positioned on a rear portion of the tow vehicle. The trailer angle and position with respect to the tow vehicle may be determined, for example, by fusing visual information from received images and IMU (inertial measurement unit) sensor data. In addition, it is desirable to have a tow vehicle that is capable of autonomously maneuvering along a planned path towards the trailer and to align with a trailer hitch coupler of the trailer for autonomous hitching between a vehicle hitch ball of the tow vehicle and the trailer hitch coupler.

Referring to <FIG>, in some implementations, a driver of a tow vehicle <NUM> wants to tow a trailer <NUM> positioned behind the tow vehicle <NUM>. The tow vehicle <NUM> may be configured to receive an indication of a trailer selection <NUM> by the driver associated with a selected trailer <NUM>, 200a-c. In some examples, the driver maneuvers the tow vehicle <NUM> towards the selected trailer <NUM>, 200a-c, while in other examples, the tow vehicle <NUM> autonomously drives towards the selected trailer <NUM>, 200a-c. The tow vehicle <NUM> may include a drive system <NUM> that maneuvers the tow vehicle <NUM> across a road surface based on drive commands having x, y, and z components, for example. As shown, the drive system <NUM> includes a front right wheel <NUM>, 112a, a front left wheel <NUM>, 112b, a rear right wheel <NUM>, 112c, and a rear left wheel <NUM>, 112d. The drive system <NUM> may include other wheel configurations as well. The drive system <NUM> may also include a brake system <NUM> that includes brakes associated with each wheel <NUM>, 112a-d, and an acceleration system <NUM> that is configured to adjust a speed and direction of the tow vehicle <NUM>. In addition, the drive system <NUM> may include a suspension system <NUM> that includes tires associates with each wheel <NUM>, 112a-d, tire air, springs, shock absorbers, and linkages that connect the tow vehicle <NUM> to its wheels <NUM>, 112a-d and allows relative motion between the tow vehicle <NUM> and the wheels <NUM>, 112a-d. The suspension system <NUM> may be configured to adjust a height of the tow vehicle <NUM> allowing a tow vehicle hitch <NUM> (e.g., a vehicle hitch ball <NUM>) to align with a trailer hitch <NUM> (e.g., trailer hitch coupler or trailer hitch cup <NUM>), which allows for autonomous connection between the tow vehicle <NUM> and the trailer <NUM>.

The tow vehicle <NUM> may move across the road surface by various combinations of movements relative to three mutually perpendicular axes defined by the tow vehicle <NUM>: a transverse axis X, a fore-aft axis Y, and a central vertical axis Z. The transverse axis x, extends between a right side and a left side of the tow vehicle <NUM>. A forward drive direction along the fore-aft axis Y is designated as F, also referred to as a forward motion. In addition, an aft or rearward drive direction along the fore-aft direction Y is designated as R, also referred to as rearward motion. When the suspension system <NUM> adjusts the suspension of the tow vehicle <NUM>, the tow vehicle <NUM> may tilt about the X axis and or Y axis, or move along the central vertical axis Z.

The tow vehicle <NUM> may include a user interface <NUM>. In some examples, the user interface <NUM> is a touch screen display <NUM> the allows the driver to make selections via the display. In other examples, the user interface <NUM> is not a touchscreen and the driver may use an input device, such as, but not limited to, a rotary knob or a mouse to make a selection. The user interface <NUM> receives one or more user commands from the driver via one or more input mechanisms or a touch screen display <NUM> and/or displays one or more notifications to the driver. The user interface <NUM> is in communication with a vehicle controller <NUM>, which is in turn in communication with a sensor system <NUM>. In some examples, the user interface <NUM> displays an image <NUM> of an environment of the tow vehicle <NUM> leading to one or more commands being received by the user interface <NUM> (from the driver) that initiate execution of one or more behaviors. In some examples, the user display <NUM> displays one or more trailer representations <NUM>, 136a-c of trailers <NUM> positioned behind the tow vehicle <NUM>. In this case, the driver makes a trailer selection <NUM> of a trailer representation <NUM>, 136a-c of a trailer <NUM>. The controller <NUM> may then determine a location LT of the trailer <NUM> associated with the trailer selection <NUM> relative to the tow vehicle <NUM>. In other examples, the controller <NUM> detects one or more trailers <NUM> and determines the position LT of each one of the trailers associated with each one of the trailer representations <NUM>, 136a-c relative to the tow vehicle <NUM>. The vehicle controller <NUM> includes a computing device (processor) <NUM> (e.g., central processing unit having one or more computing processors) in communication with non-transitory memory <NUM> (e.g., a hard disk, flash memory, random-access memory, memory hardware) capable of storing instructions executable on the computing processor(s) <NUM>.

The vehicle controller <NUM> executes a drive assist system <NUM>, which in turn includes path following behaviors <NUM>. The drive assist system <NUM> receives a planned path <NUM> from a path planning system <NUM> and executes behaviors 182a-182c that send commands <NUM> to the drive system <NUM>, leading to the tow vehicle <NUM> autonomously driving about the planned path <NUM> in a rearward direction R and autonomously hitching with the trailer <NUM>.

The path following behaviors <NUM> include, a braking behavior 182a, a speed behavior 182b, and a steering behavior 182c. In some examples, the path following behaviors <NUM> also include a hitch connect behavior (allowing the vehicle hitch ball <NUM> to connect to the trailer hitch cup <NUM>), and a suspension adjustment behavior (causing the vehicle suspension to adjust to allow for the hitching of the vehicle hitch ball <NUM> and the trailer hitch coupler <NUM>). Each behavior 182a-182c causes the tow vehicle <NUM> to take an action, such as driving backward, turning at a specific angle, breaking, speeding, slowing down, among others. The vehicle controller <NUM> may maneuver the tow vehicle <NUM> in any direction across the road surface by controlling the drive system <NUM>, more specifically by issuing commands <NUM> to the drive system <NUM>.

The tow vehicle <NUM> may include a sensor system <NUM> to provide reliable and robust driving. The sensor system <NUM> may include different types of sensors that may be used separately or with one another to create a perception of the environment of the tow vehicle <NUM> that is used for the tow vehicle <NUM> to drive and aid the driver in make intelligent decisions based on objects and obstacles detected by the sensor system <NUM>. The sensor system <NUM> may include the one or more cameras <NUM>, 142a-d. In some implementations, the tow vehicle <NUM> includes a rear camera <NUM>, 142a that is mounted to provide a view of a rear-driving path of the tow vehicle <NUM>. The rear camera 142a may include a fisheye lens that includes an ultra wide-angle lens that produces strong visual distortion intended to create a wide panoramic or hemispherical image. Fisheye cameras capture images having an extremely wide angle of view. Moreover, images <NUM> captured by the fisheye camera 142a have a convex non-rectilinear appearance. Other types of cameras <NUM> may also be used to capture the images <NUM> of the rear environment of the tow vehicle <NUM>.

In some implementations, the sensor system <NUM> may also include the IMU (inertial measurement unit) <NUM> configured to measure the vehicle's linear acceleration (using one or more accelerometers) and rotational rate (using one or more gyroscopes). In some examples, the IMU <NUM> also determines a heading reference of the tow vehicle <NUM>. Therefore, the IMU <NUM> determines the pitch, roll, and yaw of the tow vehicle <NUM>.

The sensor system <NUM> may include other sensors such as, but not limited to, radar, sonar, LIDAR (Light Detection and Ranging, which can entail optical remote sensing that measures properties of scattered light to find range and/or other information of a distant target), LADAR (Laser Detection and Ranging), ultrasonic sensors, etc. Additional sensors may also be used.

The vehicle controller <NUM> executes a location estimation and path planning system <NUM> that receives images <NUM> from the camera <NUM> and IMU data <NUM> from the IMU <NUM> and determines a location LT of the trailer <NUM> with respect to the tow vehicle <NUM> based on the received images <NUM>.

The controller <NUM> receives image data <NUM> from the rear camera 142a and IMU sensor data <NUM> from the IMU <NUM> and based on the received data <NUM>, <NUM>, the controller <NUM> estimates the camera pose <NUM> (i.e., camera location Lc and angle αC) and trailer pose <NUM> (i.e., trailer location LT and angle αT) with respect to the world coordinate origin. The world coordinate origin is defined at the starting pose of the vehicle <NUM>. The controller <NUM> includes a location estimation and path planning system <NUM> that includes a location estimation system <NUM>. The location estimation system <NUM> fuses camera data <NUM> (e.g., monocular camera data) and IMU sensor data <NUM> to estimate the trailer pose <NUM>, i.e., the trailer location LT and the trailer angle αT with respect to the world coordinate origin and a camera location Lc and camera angle αC with respect to the world coordinate. The trailer angle αT being an angle between a fore-aft axis Y of the world coordinate and the centerline of the trailer <NUM>. The camera angle αC being an angle between the fore-aft axis Y of the world coordinate and the optical axis of the camera. Once the location estimation system <NUM> determines the location LT of the trailer 200a and the angle αT of the trailer 200a with respect to the world coordinate and the camera location Lc and camera angle αC with respect to the world coordinate, then the path planning system <NUM> determines a trajectory or path <NUM> to align the tow vehicle <NUM> with the trailer 200a.

The location estimation and path planning system <NUM> includes the location estimation system <NUM> and the path planning system <NUM>. Referring to <FIG>, the location estimation system <NUM> includes an imaging module <NUM> that includes a trailer ROI detection module <NUM> and a feature detection and tracking module <NUM> In addition, the location estimation system <NUM> includes the iterated Extended Kalman filter <NUM>, a camera pose module <NUM>, and a trailer pose estimator module <NUM>.

The trailer ROI detection module <NUM> receives the image <NUM> from the rear camera 142a that includes the trailer representation <NUM>. As previously mentioned, in some examples, the image <NUM> includes one or more trailer representations <NUM>, 136a-c associated with one or more trailers <NUM>, 200a-c positioned behind the tow vehicle <NUM>. In some examples, when the image <NUM> includes more than one trailer representations <NUM>, the location estimation and path planning system <NUM> instructs the display <NUM> to solicit a trailer selection <NUM> associated with a representation of one of the trailers <NUM>. The driver selects the trailer 200a (i.e., a representation 136a of the trailer 200a) from the image <NUM>. Once the trailer representation <NUM> is selected or identified, in some examples, the trailer ROI detection module <NUM> bounds the identified trailer representation <NUM> by a bounding box also referred to as a region of interest (ROI) <NUM>, shown in <FIG>. In other examples, the trailer ROI detection module <NUM> instructs the display <NUM> to solicit the driver to select the trailer 200a (i.e., a representation 136a of the trailer 200a) by bounding the identified trailer representation 136a by the bounding box i.e., the ROI <NUM>. The driver may enter the bounding box <NUM> via the user interface <NUM>, for example, via the touch screen display <NUM>, or a rotary knob or a mouse to select the bounding box <NUM> around the trailer representation <NUM> within the image <NUM>.

Once the trailer ROI detection module <NUM> detects and identifies the ROI <NUM> that includes the trailer representation <NUM>, then a feature detection and tracking module <NUM> identifies visual feature points <NUM> inside the ROI <NUM>. The visual feature points <NUM> may be, for example, the trailer wheels, the coupler <NUM>, the trailer edges, or any other feature associated with the trailer <NUM>. Once the feature points <NUM> within the ROI <NUM> are detected, the feature detection and tracking module <NUM> tracks the detected features <NUM> while the tow vehicle <NUM> moves towards the trailer <NUM>.

As previously mentioned, the location estimation system <NUM> includes an iterated extended Kalman filter <NUM> that receives the identified feature points <NUM> within the ROI <NUM> and the IMU sensor data <NUM> and analyzes the received data <NUM>, <NUM>. The iterative extended Kalman filter <NUM> fuses the identified feature points <NUM> and the IMU data <NUM>. The iterative extended Kalman filter <NUM> uses a system's dynamic model, known control inputs to that system, and multiple sequential measurements (from sensors) to estimate the system's varying quantities.

An extended Kalman filter is a nonlinear version of the Kalman filter which linearizes about an estimate of the current mean and covariance. The Kalman filter is an algorithm that uses a series of measurements observed over time, and including statistical noise and other inaccuracies, and outputs estimates of unknown variables that are more accurate than those based on one measurement, because the Kalman filter <NUM> estimates a joint probability distribution over variables for each timeframe. The Kalman filter <NUM> executes its calculations in a two-step process. During the first step, also referred to as the prediction step, the Kalman filter <NUM> determines current states 322c (states <NUM>, 324ac-324jc), along with uncertainties associated with each current state variable 324a-j. When the outcome of the present measurement is observed, in the second step also known as the update step, these current states 314c (states <NUM>, 324ac-324jc) are updated using a weighted average, with more weight being given to the one with higher certainty (either current states 314c (states <NUM>, 324ac-324jc) or the present measurement). The algorithm is recursive, and runs in real time, using the present input measurements and the current filter states 322c (states <NUM>, 324ac-324jc) to determine updated filter states 322u (states <NUM>, 324au-324ju), thus no additional past information is needed. An iterated extended Kalman filter <NUM> improves the linearization of the extended Kalman filter by reducing the linearization error at the cost of increased computational requirement.

The iterated extended Kalman filter <NUM> receives the IMU sensor data <NUM> from the IMU <NUM> and the visual feature points <NUM> within the ROI <NUM>. In some implementations, the Iterated extended Kalman filter <NUM> determines filter states <NUM>, 322c, 322u that update continuously based on the received IMU sensor data <NUM> and the visual feature points <NUM>. The filter states <NUM>, 322c, 322u may include calibrated values such as a distance 324f between the IMU <NUM> and the rear camera 142a since the position of both within the tow vehicle <NUM> is known. In some examples, the filter states <NUM>, 322c, 322u include an IMU position state 324a, an IMU velocity state 324b, and an IMU altitude state 324c that are determined by the iterated extended Kalman filter <NUM> based on IMU sensor data <NUM> (that includes the acceleration and the angular velocity data of the tow vehicle <NUM>), the calibrated distance 324f between the IMU <NUM> and the rear camera 142a, and the position of the camera 142a in a coordinate system, for example a world coordinate system. The world coordinate system defines the world origin (a point whose coordinates are [<NUM>,<NUM>,<NUM>]) and defines three-unit axes orthogonal to each other. The coordinate of any point in the world space are defined with respect to the world origin. Once the world coordinate system is defined, the position of the camera 142a may be defined by a position in the world space and the orientation of camera 142a may be defined by three-unit vectors orthogonal to each other. In some examples, the world origin is defined by the initial position of the camera, and the three-unit axes are defined by the initial camera orientation. The position of the camera 142a is determined or known by the camera pose module <NUM> as will be described below. As previously mentioned, the IMU <NUM> includes an accelerometer for determining the linear acceleration of the tow vehicle and a gyroscope for determining the rotational rate of the vehicle wheels. In addition, in some examples, the filter states <NUM>, 322c, 322u include an Accelerometer bias state 324d and a Gyroscope bias state 324e. The inertial sensors such as the accelerometer and gyroscope often include small offset in the average signal output, even when there is no movement. The Accelerometer bias state 324d estimates the small offset of the accelerometer sensor in the average signal output, and the Gyroscope bias state 324e estimates the small offset of the gyroscope sensor in the average signal output.

In some examples, the filter states <NUM>, 322c, 322u include a camera orientation to the ROI feature points state <NUM> being the orientation of the camera 142a to the one or more ROI feature points <NUM> identified by the feature detection and tracking module <NUM>. The filter states <NUM> may also include a camera distance to ROI feature points state <NUM> being the distance between the camera 142a and the one or more ROI feature points <NUM> identified by the feature detection and tracking module <NUM>.

The iterated extended Kalman filter <NUM> generates a pixel-wise intensity difference <NUM> between the visual feature points <NUM> of a current image <NUM> and a previously tracked image <NUM>. The extended iterated Kalman filter <NUM> determines the pixel-wise intensity difference <NUM> by running a routine on a pixel location of the current image <NUM> and the previously tracked image <NUM> and returns a result, then moves to the next pixel location and repeats the same routine, until all pixels of the current image <NUM> and the previously tracked image <NUM> are processed and the pixel-wise intensity difference <NUM> is determined.

In a first step, the iterated extended Kalman filter <NUM> predicts current filter states 322c, 324ac-324jc based on the received IMU sensor data <NUM> (i.e., acceleration and angular velocity data). In a second step, the iterated extended Kalman filter <NUM> updates the values of the current states 324ac-324ic based on the pixel-wise intensity difference <NUM> between the tracked feature points <NUM> of a current image <NUM> and the previous corresponding feature points <NUM> of a previously tracked image <NUM> to update and correct the updated filter states 322u, 324au-324iu. Therefore, the current state 324ac-324ic of each of the mentioned state 322c is updated every time the iterated extended Kalman filter <NUM> receives data from the IMU <NUM> and the rear camera 142a.

The location estimation system <NUM> also includes a camera pose module <NUM> that calculates a location LC and orientation or an angle αC of the camera 142a in the world coordinate system based on the updated filter states 322u. For example, the camera pose module <NUM> calculates a camera location Lc and a camera angle αC as shown in <FIG>, the camera angle αC being an angle at the world coordinate origin between the fore-aft axis Y of the world coordinate and the position of the camera.

The trailer pose estimator module <NUM> calculates the location <NUM> of the feature points <NUM> in the world coordinate system based on the updated filter states 322u. Then the trailer pose estimator module <NUM>, for example a plane extraction module <NUM>, determines a plane P based on the feature points location <NUM>. The plane P is the front face of the trailer <NUM> as shown in <FIG>. Once the plane P is identified, then the trailer pose estimator module <NUM> calculates a normal line <NUM> to the plane P to identify the trailer angle αT with respect to the world coordinate system.

As previously discussed, the location estimation system <NUM> leverages the iterative extended Kalman filter <NUM> to fuse camera image data <NUM> and IMU sensor data <NUM> to estimate and determine the trailer location LT and the trailer angle αT in the world coordinate system, and the camera location LC and camera angle αC in the world coordinate system. The location estimation system <NUM> provides high accuracy by using tightly coupled fusion techniques and increases robustness by leveraging feature detection and tracking module <NUM> for updating current filter states 322c in the iterated extended Kalman filter <NUM>. As described above, the location estimation system <NUM> utilizes low computational resource and achieves real time performance using low cost hardware. Moreover, the location estimation system <NUM> increases the robustness of the determination of the trailer location due to reliance on the analysis of the images <NUM> by the feature detection and tracking module <NUM> for the filter state updates 322u.

Referring to <FIG>, the path planning system <NUM> includes a trajectory planning module <NUM> and a trajectory control module <NUM>. The trajectory planning module <NUM> receives the pose <NUM> (i.e., camera location LC and camera angle αC) from the camera pose module <NUM> and the trailer pose <NUM> i.e., trailer location LT and trailer angle αT) from the trailer pose estimator module <NUM>. The trajectory planning module <NUM> includes a Dubins path generation module <NUM> and a key points generation module <NUM>. The Dubins path generation module <NUM> receives the camera pose <NUM> and the trailer pose <NUM> and generates a Dubins path <NUM>. Dubins path refers to the shortest curve that connects two points in the two-dimensional Euclidean plane (i.e., x-y plane) with a constraint on the curvature of the path and with prescribed initial and terminal tangents to the path, and an assumption that the vehicle traveling the path can only travel in one direction. Dubins path <NUM> is generated by joining circular arcs of maximum curvature and straight lines. <FIG> illustrate the Dubins path. <FIG> shows a CLC, circle line circle configuration, used when the distance between the tow vehicle <NUM> and the trailer <NUM> is at a specific threshold, not the direction of the trailer <NUM> with respect to the tow vehicle <NUM>. <FIG> shows a CL, circle line configuration, used when the distance between the tow vehicle <NUM> and the trailer <NUM> is at a minimum and only one turn is needed to align the tow vehicle <NUM> and the trailer <NUM>. <FIG> shows another CLC configuration, used when the tow vehicle <NUM> is aligned with the trailer <NUM>, but with a lateral distance. Once Dubins Path generation module <NUM> generates a path <NUM>, then key points generations module <NUM> identifies a sequence of points <NUM> along the path <NUM> for the tow vehicle <NUM> to follow.

Following, a trajectory control module <NUM> determines the steering wheel angle of the tow vehicle <NUM> in a rearward direction R to follow the path <NUM>. In addition, the trajectory control module <NUM> executes an Ackermann angle calculation <NUM> based on the received sequence of points <NUM> from the key point generation module <NUM>. Ackermann steering geometry is a geometric arrangement of linkages in the steering of the tow vehicle <NUM> that solves the problem of wheels <NUM> on the inside and outside of a turn needing to trace out circles of different radii. Therefore, Ackermann angle calculation <NUM> assumes that all the wheels <NUM> have their axles arranged as radii of circles with a common center point. As the rear wheels are fixed, this center point must be on a line extended from the rear axle. Intersecting the axes of the front wheels on this line as well requires that the inside front wheel be turned, when steering, through a greater angle than the outside wheel. Therefore, Ackermann angle calculation <NUM> determines difference between the steering angle of each front wheel. The trajectory control module <NUM> updates the steering wheel angles based on the Ackermann angle calculation <NUM> and determines an updated steering wheel angle <NUM> for each one of the front wheels 112a, 112b to follow the path <NUM>.

As the tow vehicle <NUM> is autonomously maneuvering along the planned path <NUM>, the path planning system <NUM> continuously updates the path based on continuously receiving sensor data. In some examples, an object detection system identifies one or more objects along the planned path and sends the path planning system <NUM> data relating to the position of the one or more objects. In this case, the path planning system <NUM> recalculates the planned path <NUM> to avoid the one or more objects while also executing the predetermined maneuvers. In some examples, the path planning system determines a probability of collision and if the probability of collision exceeds a predetermined threshold, the path planning system <NUM> adjusts the path <NUM> and sends it to the drive assist system <NUM>.

Once the path planning system <NUM> determines the planned path <NUM>, then the vehicle controller <NUM> executes the drive assist system <NUM>, which in turn includes path following behaviors <NUM>. The path following behaviors <NUM> receive the planned path and executes one or more behaviors 182a-b that send commands <NUM> to the drive system <NUM>, causing the tow vehicle <NUM> to autonomously drive along the planned path, which causes the tow vehicle <NUM> to autonomously drive along the planned path.

The path following behaviors 182a-b may include one or more behaviors, such as, but not limited to, a braking behavior 182a, a speed behavior 182b, and a steering behavior 182c. Each behavior 182a-b causes the tow vehicle <NUM> to take an action, such as driving backward, turning at a specific angle, breaking, speeding, slowing down, among others. The vehicle controller <NUM> may maneuver the tow vehicle <NUM> in any direction across the road surface by controlling the drive system <NUM>, more specifically by issuing commands <NUM> to the drive system <NUM>.

The braking behavior 182a may be executed to either stop the tow vehicle <NUM> or to slow down the tow vehicle <NUM> based on the planned path. The braking behavior 182a sends a signal or command <NUM> to the drive system <NUM>, e.g., the brake system (not shown), to either stop the tow vehicle <NUM> or reduce the speed of the tow vehicle <NUM>.

The speed behavior 182b may be executed to change the speed of the tow vehicle <NUM> by either accelerating or decelerating based on the planned path. The speed behavior 182b sends a signal or command <NUM> to the brake system <NUM> for decelerating or the acceleration system <NUM> for accelerating.

The steering behavior 182c may be executed to change the direction of the tow vehicle <NUM> based on the planned path. As such, the steering behavior 182c sends the acceleration system <NUM> a signal or command <NUM> indicative of an angle of steering causing the drive system <NUM> to change direction.

As previously discussed, the location estimation and path planning system <NUM> provides the location of a selected trailer <NUM> in the world coordinate system without the use of one or more markers positioned on the trailer <NUM>. Moreover, the location estimation and path planning system <NUM> provides high accuracy by using tightly coupled fusion techniques, which require low computational resources, and which can achieve real time performance on low cost hardware. Since no marker is needed on the trailer <NUM>, the complexity of the operation and computations is reduced and thus the production cost of the system is also reduced.

<FIG> provides an example arrangement of operations for a method <NUM> of autonomously maneuvering a tow vehicle <NUM> towards a trailer <NUM> for autonomous hitching between the trailer <NUM> and the tow vehicle <NUM> using the system described in <FIG>. At block <NUM>, the method <NUM> includes receiving, at a data processing hardware <NUM>, images from one or more cameras 142a positioned on a back portion of the tow vehicle <NUM> and in communication with the data processing hardware <NUM>. The camera may include a fisheye lens that includes an ultra wide-angle lens. At block <NUM>, the method <NUM> includes receiving, at the data processing hardware <NUM>, sensor data <NUM> from an inertial measurement unit <NUM> in communication with the data processing hardware <NUM> and supported by the tow vehicle <NUM>. At block <NUM>, the method <NUM> includes determining, at the data processing hardware <NUM>, a pixel-wise intensity difference <NUM> between a current received image <NUM> and a previous received image <NUM>. At block <NUM>, the method <NUM> includes determining, at the data processing hardware <NUM>, a camera pose <NUM> and a trailer pose <NUM> both with respect to a world coordinate system. The camera pose <NUM> and the trailer pose <NUM> are based on the images <NUM>, the sensor data145, and the pixel-wise intensity difference <NUM>. At block <NUM>, the method <NUM> includes determining, at the data processing hardware <NUM>, a tow vehicle path <NUM> based on the camera pose <NUM> and the trailer pose <NUM>. At block <NUM>, the method <NUM> includes sending instruction <NUM> from the data processing hardware <NUM> to a drive system <NUM> supported by the tow vehicle <NUM>. The instructions <NUM> causing the tow vehicle <NUM> to autonomously maneuver along the tow vehicle path <NUM> in a reverse direction R so that the tow vehicle <NUM> hitches with the trailer <NUM>.

In some example, the camera pose includes a camera location LC and a camera angle αC in the world coordinate system. The trailer pose may include a trailer location LC and a trailer angle αT in the world coordinate system. The method <NUM> may include identifying one or more feature points within the images, the feature points <NUM> associated with the trailer <NUM>. The method <NUM> may also include determining a plane P based on the one or more feature points <NUM>. The plane P is indicative of a front face of the trailer <NUM>. The method may include determining a normal line to the plane P and determining the trailer angle αT based on the normal line.

In some examples, the method includes determining a region of interest <NUM> surrounding a representation <NUM> of the trailer <NUM> within the images <NUM>. Determining a pixel-wise intensity difference <NUM> between the current received image <NUM> and a previous received image <NUM>, includes determining the pixel-wise intensity difference <NUM> between the region of interest <NUM> of the current received image <NUM> and the region of interest <NUM> of the previous received image <NUM>. The method <NUM> may further include identifying one or more feature points <NUM> within the region of interest <NUM>. The feature points <NUM> are associated with the trailer <NUM>. The method <NUM> may also include tracking the one or more feature points <NUM> as the vehicle <NUM> autonomously maneuvers along the path <NUM>.

In some implementations, the method <NUM> includes determining a sequence of points associated with the path <NUM>. The tow vehicle <NUM> follows the sequence of points along the path <NUM>. The method <NUM> also includes determining a steering wheel angle associated with each point within the sequence of points and sending instructions to the drive system <NUM> to adjust a current steering wheel angle based on the determined steering wheel angle.

The method <NUM> may also include determining a difference between a steering angle of each front wheel <NUM> of the tow vehicle <NUM>, as the vehicle <NUM> autonomously maneuver along the path <NUM>. The method <NUM> may also include sending instructions <NUM> to the drive system <NUM> to adjust the steering wheel angle of each wheel <NUM> based on the difference between the steering wheel angle of each front wheel <NUM>.

As used herein, the terms "machine-readable medium" and "computer-readable medium" refer to any computer program product, apparatus and/or device (e.g., magnetic discs, optical disks, memory, Programmable Logic Devices (PLDs)) used to provide machine instructions and/or data to a programmable processor, including a machine-readable medium that receives machine instructions as a machine-readable signal.

Implementations of the subject matter and the functional operations described in this specification can be implemented in digital electronic circuitry, or in computer software, firmware, or hardware, including the structures disclosed in this specification and their structural equivalents, or in combinations of one or more of them. Moreover, subject matter described in this specification can be implemented as one or more computer program products, i.e., one or more modules of computer program instructions encoded on a computer readable medium for execution by, or to control the operation of, data processing apparatus. The computer readable medium can be a machine-readable storage device, a machine-readable storage substrate, a memory device, a composition of matter effecting a machine-readable propagated signal, or a combination of one or more of them. The terms "data processing apparatus", "computing device" and "computing processor" encompass all apparatus, devices, and machines for processing data, including by way of example a programmable processor, a computer, or multiple processors or computers. A propagated signal is an artificially generated signal, e.g., a machine-generated electrical, optical, or electromagnetic signal that is generated to encode information for transmission to suitable receiver apparatus.

In certain circumstances, multi-tasking and parallel processing may be advantageous.

Claim 1:
A method for autonomously maneuvering a tow vehicle (<NUM>) towards a trailer (<NUM>) for autonomous hitching between the trailer (<NUM>) and the tow vehicle (<NUM>), the method comprising:
receiving, at a data processing hardware (<NUM>), images (<NUM>) from one or more cameras (142a) positioned on a back portion of the tow vehicle (<NUM>) and in communication with the data processing hardware (<NUM>);
receiving, at the data processing hardware (<NUM>), sensor data (<NUM>) from an inertial measurement unit (<NUM>) in communication with the data processing hardware (<NUM>) and supported by the tow vehicle (<NUM>);
sending, from the data processing hardware to a drive system (<NUM>) supported by the tow vehicle (<NUM>), instruction (<NUM>) to autonomously maneuver the tow vehicle (<NUM>) along a tow vehicle path (<NUM>) in a reverse direction (R) causing the tow vehicle (<NUM>) to hitch with the trailer;
characterized by that the method further comprises:
determining, at the data processing hardware (<NUM>), a pixel-wise intensity difference (<NUM>) between a current received image (<NUM>) and a previous received image (<NUM>);
determining, at the data processing hardware (<NUM>), a camera pose (<NUM>) and a trailer pose (<NUM>) with respect to a world coordinate system, the camera pose (<NUM>) and the trailer pose (<NUM>) based on the images (<NUM>), the sensor data (<NUM>), and the pixel-wise intensity difference (<NUM>); and
determining, at the data processing hardware (<NUM>), the tow vehicle path (<NUM>) based on the camera pose (<NUM>) and the trailer pose (<NUM>).