Atty Docket No. OU-24009WO PATH PLANNING AND OBSTACLE AVOIDANCE FOR PARKING AN N-TRAILER VEHICLE RELATED APPLICATION [0001] This application is a non-provisional of, and claims priority to, US Provisional Application Serial No. 63/568,306, filed March 21, 2024, the contents of which is incorporated herein in its entirety. FIELD OF THE INVENTION [0002] The present invention relates to autonomous parking and obstacle avoidance guidance for vehicles, and in particular, for N-trailer vehicles in a confined environment. BACKGROUND OF THE INVENTION [0003] Obstacle avoidance in the context of vehicle parking is a highly desirable feature with countless applications and yet is among the greatest of the challenges to autonomous ground vehicles and mobile robots. Two key technical issues arise in the context of autonomous obstacle avoidance and parking: (1) how to generate a feasible path from an initial vehicle pose (position and orientation) to a final pose without colliding with an obstacle given the non-holonomic kinematic constraints of a wheeled vehicle (i.e. the wheels cannot move sideways), and (2) how to generate a feasible velocity profile for the vehicle to traverse the path at the highest speed having acceptable position errors given the dynamics constraints (i.e. mass and force properties). Parallel parking and backing up a vehicle with a trailer are good examples that demonstrate some of these technical challenges. [0004] Others have attempted to solve some of the problems in this area. For example, the background in U.S. Pat. No. 11,904,938 (the ‘938 Patent) discusses a number of existing approaches, including (1) a geometric approach, (2) a three-step approach, (3) a sampling-based approach, and (4) a numerical optimization approach. The ‘938 Patent also discusses deficiencies and/or limitations of these approaches. The ‘938 Patent then discloses a four-step approach for autonomously parking a vehicle. In phases one and two, a reverse path may be determined by
Atty Docket No. OU-24009WO moving a virtual vehicle from a parking space and steering it to a target line in a drive aisle. In phase three, a forward path may be generated by steering the virtual vehicle from the initial pose to the target line. In phase four, the two paths may be connected along the target line to define a virtual path that can be used to generate a trajectory for the physical vehicle. [0005] While this approach is useful for standard vehicles, other vehicles, such as N-trailer vehicles, have more complex motion paths. An N-trailer vehicle provides an efficient, economical, and flexible way to increase the transport capacity in ground transportation by hingedly connecting N number of trailers to a vehicle. N-trailer vehicles are difficult to park due to nonholonomic constraints which confine the instantaneous control direction of the vehicle, while also having high degrees of freedom relative to standard (e.g., 0-trailer) vehicles. The path needed to park an N- trailer vehicle is complex due to problems such as jackknifing. Jackknifing is an uncontrollable folding of a multi-trailer vehicle where the trailer(s) swing to extreme angles (e.g., angles <= 90∘) relative to the towing vehicle. This can result in loss of control, vehicle damage, and can cause accidents. Jackknifing can occur due to instability when reversing the vehicle and N-trailer(s), due to a high hitch angle, sudden breaking, slippery or uneven roads, and due to poor path planning. [0006] In “Nonholonomic motion planning: Steering using sinusoids”, the term chained form was introduced, which refers to a class of equations for driftless systems. See, e.g., R. M. Murray and S. S. Sastry, “Nonholonomic motion planning: Steering using sinusoids”, IEEE Transactions on Automatic Control, vol. 38, no. 5, pp.700-716, 1993. In “Conversion of the kinematics of a car with n trailers into a chained form”, and in “Trajectory generation for the n-trailer problem using Goursat normal form”, it was shown that the standard N-trailer system can be converted into the chained formed. See, e.g., O. Sordalen, “Conversion of the kinematics of a car with n trailers into a chained form”, 1993. Proceedings., 1993 IEEE International Conference. IEEE, 1993, pp. 382-387; and D. Tilbury, R. M. Murray, and S.S. Sastry, “Trajectory generation for the n-trailer problem using Goursat normal form”, IEEE Transaction on Automatic Control, vol.40, no.5, pp. 802-819, 1995. For example, in “Trajectory generation for the n-trailer problem using Goursat normal form”, chained form conversion is used to plan analytic parking paths for standard 1- and 2-trailers. See Id. These approaches did not consider or introduce path planning when obstacles may be present to interfere with the ideal path of the vehicle. In “Topological property for
Atty Docket No. OU-24009WO collision-free nonholonomic motion planning: The case of sinusoidal inputs for chained form systems”, and in “Multilevel path planning for nonholonomic robots using semi holonomic subsystems”, a multi-step approach was developed for path planning in the presence of obstacles. See, e.g., S. Sekhavat and J.P. Laumond, “Topological property for collision-free nonholonomic motion planning: The case of sinusoidal inputs for chained form systems”, IEEE Transactions on Robotics and Automation, vol. 14, no. 5, pp. 671-680, 1998; and S. Sekhavat, P. Svestka, J.P. Laumond, and M. H. Overmars, “Multilevel path planning for nonholonomic robots using semi holonomic subsystems”, The international journal of robotics research, vol.17, no.8, pp.840-857, 1998. [0007] Although a kinematic trajectory of an N-trailer can be computed analytically after being put into chained form, the explicit geometric meanings of the kinematics are lost in the conversion. Thus, the chained form conversion based approach has difficulties with path planning when obstacles are present. Further, the approach is not able to address the jack knifing phenomenon. Furthermore, while the multi-step approach can be used to identify a collision-free path in an environment with obstacles, it does not consider the problem of jackknifing in its design. Another drawback of the multi-step approach is high computational cost, making the tool useful for offline, rather than real-time, assessments. [0008] In some situations, trailers may not be hitched directly at the midpoint of the rear axle, and instead, may be hitched in an offset position. One example of this is a car pulling a trailer or articulated farm vehicle. Offset hitching makes the path planning problem difficult to address analytically. Further, it is known that general N-trailers are not flat and thus not chained-form convertible when there are two or more trailers. As shown in “Some properties of the general n- trailer”, general N-trailer systems can be put into a multi-input chained-form with virtual feedback. See, e.g., C. Altafini, “Some properties of the general n-trailer”, International Journal of Control, vol.74, no.4, pp.409-424, 2001. However, this transformation is not useful in practice because the explicit expression for the feedback in the new coordinates is difficult (and computationally expensive) to obtain. [0009] Path planning for parking general N-trailers is still an open problem. In “Motion planning for a reversing general 2-trailer configuration using Closed-Loop RRT”, a path planner
Atty Docket No. OU-24009WO for reversing a general 2-trailer vehicle with an off-axle hitch connection is provided. See, e.g., N. Evestedt, O. Ljungqvist, and D. Axehill, “Motion planning for a reversing general 2-trailer configuration using closed-loop rrt”, in Intelligent Robots and Systems (IROS), 2016 IEEE/RSJ International Conference. IEE, 2016, pp. 3690-3697. Another Closed-Loop Rapidly Exploring Random Tree (CL-RRT) framework is described in “Real-time motion planning with applications to autonomous urban driving”. See, e.g., Y. Kuwata, J. Teo, G. Fiore, S. Karaman, E. Frazzoli, and J.P. How, “Real-time motion planning with applications to autonomous urban driving”, IEEE Transactions on Control Systems Technology, vol. 17, no. 5, pp. 1105-1118, 2009. In this CL- RRT framework, a cascaded controller that consists of a linear quadratic controller and a pure pursuit follower is used in the forward integration of candidate paths. Another approach involves Lattice-based motion planning for a general 2-trailer system. See, e.g., O. Ljungqvist, N. Evestedt, M. Cirillo, D. Axehill, and O. Holmer, “Lattice-based motion planning for a general 2-trailer system”, in Intelligent Vehicles Symposium (IV), 2017 IEEE. IEEE, 2017, pp.819-824. [0010] The CL-RRT approach often generates a curvy path and has difficulty in a restrictive area where directional changes may be necessary. The lattice-based approach requires a pre- calculation of the lattice which leads to long computational times that increase exponentially with the dimensions of the configuration space (i.e., the degrees of freedom of the system). [0011] It is an object of the present invention to overcome one or more of the problems described above. SUMMARY OF THE INVENTION [0012] In an aspect of the invention, a device is provided. The device includes one or more memories and one or more processors, communicatively coupled to the one or more memories. The one or more processors are to define a virtual space containing virtual objects including a vehicle, a trailer hingedly connected to the vehicle, a parking space, an obstacle corner, and a target line in a drive aisle adjacent to the parking space. The one or more processors are further to determine, using a path planning technique, a first virtual path for the trailer between a first trailer pose in the parking space and a final trailer pose outside of the parking space. The one or more processors are further to determine a second virtual path for the vehicle between a first vehicle
Atty Docket No. OU-24009WO pose in the parking space and a final vehicle pose outside of the parking space. The second virtual path is determined by using kinematic parameters to relate movement of the trailer when moving along the first virtual path to movement of the vehicle. The one or more processors are further to generate one or more control signals that cause a physical vehicle connected to a physical trailer to move in a physical space. Movement of the physical vehicle corresponds to the second virtual path and movement of the physical trailer corresponds to the first virtual path. [0013] In an embodiment of the invention, the one or more processors, when determining the first virtual path for the trailer, are to determine a ready position from which the trailer avoids the obstacle corner when moving along a path between the ready position and the target line. The one or more processors are further to move the trailer between the first trailer pose in the parking space and a second trailer pose in the ready position. The one or more processors are further to move the trailer between the second trailer pose and a third trailer pose on the target line. The one or more processors are further to determine a first path segment by tracing the trailer as the trailer is moved between the first trailer pose, the second trailer pose, and the third trailer pose. The one or more processors are further to move the trailer between the first trailer pose and a fourth trailer pose on the target line. The one or more processors are further to determine a second path segment by tracing the trailer as the trailer is moved between the fourth trailer pose and a final trailer pose. The one or more processors are further to connect the first path segment to the second path segment to define the first virtual path between the first trailer pose and the final trailer pose. [0014] In another embodiment of the invention, the one or more processors, when determining the second virtual path for the vehicle, are to determine the first vehicle pose in the parking space. The one or more processors are further to determine, periodically while the trailer is moving between positions along the first virtual path, an orientation ^^ of the vehicle based on an orientation ^^ of the trailer and a hitch angle ^^ representing a difference between the orientation ^^ of the and the orientation ^^ of the vehicle. The one or more processors are further to determine, periodically while the trailer is moving between positions along the first virtual path, a position (^^, ^^) of the vehicle based on a position (^^, ^^) of the trailer, the orientation ^^ of the trailer, and a length ^^ from a hitch point of the vehicle to an axle of the trailer. The
vehicle pose is defined
a final orientation ^^ and a final position (^^, ^^) of the vehicle.
Atty Docket No. OU-24009WO [0015] In another embodiment of the invention, rear wheels of the vehicle are treated as steering wheels for the trailer, and a hitch angle ^^, which represents a relative angle between the trailer and the vehicle, is treated as a steering for the trailer. In this embodiment, the one or
more processors, when determining the first path, are to determine, periodically while the trailer is moving between positions along the first virtual path, a curvature ^^ of a portion of the first virtual path to which the trailer has traversed at each respective time step. The one or more processors are further to determine whether the curvature ^^ exceeds a jackknife prevention threshold. Curvature values above the jackknife prevention threshold are capable of causing a jackknifing phenomenon to occur. The one or more processors are further to adjust the curvature ^^ if a curvature value is determined to be above the jackknifing prevention threshold such that the jackknifing phenomenon does not occur. [0016] In another embodiment of the invention, the one or more processors, when determining the first virtual path, are to determine, periodically while the trailer is moving between positions along the first virtual path, a steering angle δ of the vehicle. The steering angle δ is determined using a kinematic equation that expresses the steering angle δ in terms of a curvature ^^ of the trailer and a steering constraint that limits a speed at which the trailer is capable of changing direction. The one or more processors are further to adjust the steering angle δ of the vehicle if the steering angle δ is not within a permissible range defined by an upper bound steering angle maximum and a lower bound steering angle minimum. [0017] In another embodiment of the invention, the trailer is a final trailer that is one of multiple trailers each hingedly connected to the vehicle or to a preceding trailer in a sequential manner. In this embodiment, the one or more processors are further to determine a third virtual path for a penultimate trailer that is adjacent to the final trailer. The third virtual path includes an initial trailer pose in the parking space and a final trailer pose of the penultimate trailer while outside of the parking space. Referring to the same embodiment, the one or more processors, when generating the one or more control signals, are to generate the one or more control signals to cause the physical vehicle, which is connected to a penultimate physical trailer that is connected to a final physical trailer, to move in the physical space. [0018] In another embodiment of the invention, the one or more processors, when determining
Atty Docket No. OU-24009WO the final trailer pose of the penultimate trailer, are to determine, periodically while the final trailer moves between positions along the first virtual path, an orientation ^^^^ of the penultimate trailer based on an orientation ^^ of the final trailer, and a hitch angle ^^^^ representing a difference between the orientation ^^ of the final trailer and the orientation ^^^^ of the penultimate trailer.
The one or more processors are further to determine, the final trailer is moving
between positions along the first virtual path, a position of the penultimate trailer based on a position (^^, ^^) of the final trailer, the orientation ^^ of the final trailer, and a length ^^ from a hitch point of the vehicle to an axle of the trailer. The final trailer pose of the penultimate trailer is defined using a final orientation ^^^^ value and a final position (^^^^, ^^^^) value. [0019] In another embodiment of the invention, the one or more processors, when determining the first virtual path, are to determine, periodically while the trailer is moving between positions along the first virtual path, a curvature ^^ of a portion of the first virtual path to which the trailer has traversed at each respective time step. The one or more processors are further to determine whether the curvature ^^ exceeds a jackknife prevention threshold, where curvature values above the jackknife prevention threshold are capable of causing a jackknifing phenomenon to occur. The one or more processors are further to adjust the curvature ^^ if a curvature value is determined to be above the threshold curvature level such that the jackknifing phenomenon does not occur. [0020] In another aspect of the invention, a method is provided. The method includes defining a virtual space containing virtual objects including a vehicle, a trailer hingedly connected to the vehicle, a parking space, an obstacle corner, and a target line in a drive aisle adjacent to the parking space. The method further includes determining a first virtual path for the trailer between a first trailer pose in the parking space and a final trailer pose outside of the parking space. The method further includes determining a second virtual path for the vehicle between a first vehicle pose in the parking space and a final vehicle pose outside of the parking space. The second virtual path is determined by using kinematic parameters to relate movement of the trailer when moving along the first virtual path to movement of the vehicle. The method further includes generating one or more control signals that cause a physical vehicle connected to a physical trailer to move in a physical space. Movement of the physical vehicle corresponds to the second virtual path and movement of the physical trailer corresponds to the first virtual path.
Atty Docket No. OU-24009WO [0021] In an embodiment of the invention, when determining the first virtual path for the trailer, the method includes determining a ready position from which the trailer avoids the obstacle corner when moving along a path between the ready position and the target line. The method further includes moving the trailer between the first trailer pose in the parking space and a second trailer pose in the ready position. The method further includes moving the trailer between the second trailer pose and a third trailer pose on the target line. The method further includes determining a first path segment by tracing the trailer as the trailer is moved between the first trailer pose, the second trailer pose, and the third trailer pose. The method further includes moving the trailer between the first trailer pose and a fourth trailer pose on the target line. The method further includes determining a second path segment by tracing the trailer as the trailer is moved between the fourth trailer pose and a final trailer pose. The method further includes connecting the first path segment to the second path segment to define the first virtual path between the first trailer pose and the final trailer pose. [0022] In another embodiment of the invention, when determining the second virtual path for the vehicle, the method includes determining the first vehicle pose in the parking space. The method further includes determining, periodically while the trailer is moving between positions along the first virtual path, an orientation ^^ of the vehicle based on an orientation ^^ of the trailer and a hitch angle ^^ representing a difference between the orientation ^^ of
trailer and the orientation ^^ of the vehicle. The method further includes determining, periodically while the trailer is moving between positions along the first virtual path, a position (^^, ^^) of the vehicle based on a position (^^, ^^) of the trailer, the orientation ^^ of the trailer, and a length ^^ from a hitch point of the vehicle to an axle of the trailer. The final vehicle pose is defined using a final orientation ^^ and a final position (^^, ^^) of the vehicle. [0023] In another embodiment of the invention, when determining the first virtual path, the method includes determining, periodically while the trailer is moving between positions along the first virtual path, a curvature ^^ of a portion of the first virtual path to which the trailer has traversed at each respective time step. The method further includes determining whether the curvature ^^ exceeds a jackknife prevention threshold. Curvature values above the jackknife prevention threshold are capable of causing a jackknifing phenomenon to occur. The method
Atty Docket No. OU-24009WO further includes adjusting the curvature ^^ if a curvature value is determined to be above the jackknifing prevention threshold such that the jackknifing phenomenon does not occur. [0024] In another embodiment of the invention, when determining the first virtual path, the method includes determining, periodically while the trailer is moving between positions along the first virtual path, a steering angle δ of the vehicle. The steering angle δ is determined using a kinematic equation that expresses the steering angle δ in terms of a curvature ^^ of the trailer and a steering constraint that limits a speed at which the trailer is capable of changing direction. The method further includes adjusting the steering angle δ of the vehicle if the steering angle δ is not within a permissible range defined by an upper bound steering angle maximum and a lower bound steering angle minimum. [0025] In another embodiment of the invention, the trailer is a final trailer that is one of multiple trailers each hingedly connected to the vehicle or to a preceding trailer in a sequential manner. In this embodiment, the method further includes determining a third virtual path for a penultimate trailer that is adjacent to the final trailer, the third virtual path including an initial trailer pose in the parking space and a final trailer pose of the penultimate trailer while outside of the parking space. In this embodiment, when generating the one or more control signals, the method includes generating the one or more control signals to cause the physical vehicle connected to the physical trailer and to one or more additional physical trailers to move in the physical space. [0026] In another embodiment of the invention, when determining the final trailer pose of the penultimate trailer, the method includes determining, periodically while the final trailer moves between positions along the first virtual path, an orientation ^^^^ of the penultimate trailer based on an orientation ^^ of the final trailer, and a hitch angle
representing a difference between the orientation ^^ of the final trailer and the orientation
of the penultimate trailer. In this embodiment, the method includes determining, periodically while the final trailer is moving between positions along the first virtual path, a position (^^^^, ^^^^) of the penultimate trailer based on a position (^^, ^^) of the final trailer, the orientation ^^ of the final trailer, and a length ^^ from a hitch point of the vehicle to an axle of the trailer. The final trailer pose of the penultimate trailer is defined using a final orientation ^^^^ value and a final position (^^^^, ^^^^) value.
Atty Docket No. OU-24009WO [0027] In another embodiment of the invention, when determining the first virtual path, the method includes determining, periodically while the trailer is moving between positions along the first virtual path, a curvature ^^ of a portion of the first virtual path to which the trailer has traversed at each respective time step. The method further includes determining whether the
curvature ^^ exceeds a prevention threshold, where curvature values above the jackknife prevention threshold are capable of causing a jackknifing phenomenon to occur. The method further includes adjusting the curvature ^^ if a curvature value is determined to be above the threshold curvature level such that the jackknifing phenomenon does not occur. [0028] In another aspect of the invention, A non-transitory, computer-readable medium storing instructions is provided. The instructions include one or more instructions that, when executed by one or more processors, cause the one or more processors to define a virtual space containing virtual objects including a vehicle, a trailer hingedly connected to the vehicle, a parking space, an obstacle corner, and a target line in a drive aisle adjacent to the parking space. The one or more instructions further cause the one or more processors to determine, using a path planning technique, a first virtual path for the trailer between a first trailer pose in the parking space and a final trailer pose outside of the parking space. The one or more instructions further cause the one or more processors to determine a second virtual path for the vehicle between a first vehicle pose in the parking space and a final vehicle pose outside of the parking space. The second virtual path is determined by using kinematic parameters to relate movement of the trailer when moving along the first virtual path to movement of the vehicle. The one or more instructions further cause the one or more processors to generate one or more control signals that cause a physical vehicle connected to a physical trailer to move in a physical space. Movement of the physical vehicle corresponds to the second virtual path and movement of the physical trailer corresponds to the first virtual path. [0029] In an embodiment of the invention, the one or more instructions, that cause the one or more processors to determine the second virtual path for the vehicle, cause the one or more processors to determine the first vehicle pose in the parking space. The one or more instructions further cause the one or more processors to determine, periodically while the trailer is moving between positions along the first virtual path, an orientation ^^ of the vehicle based on an
Atty Docket No. OU-24009WO orientation ^^ of the trailer and a hitch angle ^^ representing a difference between the orientation ^^ of the trailer and the orientation ^^ of the vehicle. The one or more instructions further cause the one or more processors to determine, periodically while the trailer is moving between positions along the first virtual path, a position (^^, ^^) of the vehicle based on a position (^^, ^^) of the trailer, the orientation ^^ of the trailer, and a length ^^ from a hitch point of the vehicle to an axle of the trailer. The vehicle pose is defined using a final orientation ^^ and a final position
(^^, ^^) of the [0030] In another embodiment of the invention, the trailer is a final trailer that is one of multiple trailers each hingedly connected to the vehicle or to a preceding trailer in a sequential manner. In this embodiment, the one or more instructions further cause the one or more processors to determine a third virtual path for a penultimate trailer that is adjacent to the final trailer, the third virtual path including an initial trailer pose in the parking space and a final trailer pose of the penultimate trailer while outside of the parking space. In this embodiment, the one or more instructions, that cause the one or more processors to generate the one or more control signals, cause the one or more processors to generate the one or more control signals to cause the physical vehicle connected to the physical trailer and to one or more additional physical trailers to move in the physical space. [0031] In another embodiment of the invention, the one or more instructions, that cause the one or more processors to determine the final trailer pose of the penultimate trailer, cause the one or more processors to determine, periodically while the final trailer moves between positions along the first virtual path, an orientation ^^^^ of the penultimate trailer based on an orientation ^^ of the final trailer, and a hitch angle
representing a difference between the orientation ^^ of the final trailer and the orientation
of the penultimate trailer. The one or more instructions further cause the one or more
to determine, periodically while the final trailer is moving between positions along the first virtual path, a position (^^^^, ^^^^) of the penultimate trailer based on a position (^^, ^^) of the final trailer, the orientation ^^ of the final trailer, and a length ^^ from a hitch point of the vehicle to an axle of the trailer. The final trailer pose of the penultimate trailer is defined using a final orientation ^^^^ value and a final position (^^^^, ^^^^) value. [0032] The above and other objects and advantages of the present invention shall be made
Atty Docket No. OU-24009WO apparent from the accompanying drawings and the description thereof. BRIEF DESCRIPTION OF THE DRAWINGS [0033] Fig. 1 is a schematic view of a vehicle control system including a mission trajectory planner, a trajectory tracking controller, a vehicle model, and a navigation system, in accordance with the principles of the present disclosure. [0034] Fig.2 is a schematic view of the mission trajectory planner of Fig.1 including a path planner and a path-trajectory converter. [0035] Fig.3 is a diagrammatic view of coordinate frames for a 1-trailer vehicle. [0036] Fig.4 is a diagrammatic view showing a path of the 1-trailer vehicle when being moved out of a parking space. [0037] Fig.5 is a diagrammatic view of coordinate frames for an N-trailer vehicle that consists of a vehicle, a first trailer, a penultimate (N-1) trailer, and a final (N) trailer. [0038] Fig. 6A is a top down view of a 1-trailer vehicle, where motion over time is shown while the 1-trailer vehicle performs a parallel parking maneuver. [0039] Fig.6B shows a representation of the path taken by the 1-trailer vehicle of Fig.6A. [0040] Fig. 7A shows a graph plotting a hitch angle relative to a distance traveled over the course of the parallel parking maneuver shown in Fig.6A. [0041] Fig.7B shows a graph plotting a steering angle relative to a distance traveled over the course of the parallel parking maneuver shown in Fig.6A. [0042] Fig. 8A is a top down view of a 1-trailer vehicle, where motion over time is shown while the 1-trailer vehicle performs a perpendicular parking maneuver. [0043] Fig.8B shows a representation of the path taken by the 1-trailer vehicle of Fig.8A. [0044] Fig. 9A shows a graph plotting a hitch angle relative to a distance traveled over the
Atty Docket No. OU-24009WO course of the perpendicular parking maneuver shown in Fig.8A. [0045] Fig.9B shows a graph plotting a steering angle relative to a distance traveled over the course of the perpendicular parking maneuver shown in Fig.8A. [0046] Fig. 10 is a schematic view of an exemplary computing system that may be used to implement one or more of the controllers, modules, or systems described herein, and/or may be used to plan a path in accordance with the principles of the present disclosure. [0047] Fig.11 is a flowchart of an example process for computing a virtual path of a virtual 1-trailer vehicle and for using the virtual path to move a corresponding physical 1-trailer vehicle in a physical environment. [0048] The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments of the invention and, together with a general description of the invention given above, and the detailed description of the embodiments given below, serve to explain the principles of the invention. DETAILED DESCRIPTION OF THE INVENTION [0049] One or more specific embodiments of the present invention are described below. In an effort to provide a concise description of these embodiments, all features of an actual implementation may not be described in the specification. It should be appreciated that in the development of any such actual implementation, as in any engineering or design project, numerous implementation-specific decisions may be made to achieve the developers' specific goals, such as compliance with system-related and business-related constraints, which may vary from one implementation to another. Moreover, it should be appreciated that such a development effort might be complex and time consuming, but would nevertheless be a routine undertaking of design, fabrication, and manufacture for those of ordinary skill having the benefit of this disclosure. [0050] Certain exemplary aspects of the invention are set forth below. It should be understood that these aspects are presented merely to provide the reader with a brief summary of certain forms the invention might take and that these aspects are not intended to limit the scope of the invention. Indeed, the invention may encompass a variety of aspects that may not be explicitly set forth below.
Atty Docket No. OU-24009WO [0051] One or more embodiments described herein involve generating a feasible, collision- free virtual path for an N-trailer vehicle such that the virtual path can be used to autonomously or semi-autonomously park the N-trailer vehicle. In that regard, certain aspects of the present invention include a path planning technique that enables wheeled ground vehicle parking and driving around obstacles without human intervention. The technique may be implemented using a computer system onboard of the N-trailer vehicle with one or more motion sensors, situational awareness sensors, and suitable steering and speed actuators. The computer system may use a local pathway and obstacle map generated by the situational awareness sensor, or received from an off-board information source, and may generate feasible paths or trajectories. These paths or trajectories may be provided to a trajectory tracking controller to park the vehicle in tight spaces or to drive through a tightly cluttered obstacle course promptly and smoothly with minimal maneuvers, time, computational power, and human intervention. [0052] Fig. 1 is a schematic view of a vehicle control system 10 in accordance with the principles of the present disclosure. The vehicle control system 10 may include a mission trajectory planner 12, a trajectory tracking controller 14, a navigation system 16, and a vehicle model 18 that receives control signals 20 from the trajectory tracking controller 14. The control signals 20 may include, for example, one or more signals indicative of a steering angle, throttle, and brake input. Based on mission objective data 22 and situational awareness data 24, the mission trajectory planner 12 may generate data defining a feasible, collision-free nominal trajectory 26 configured to lead an N-trailer vehicle to a fixed target (e.g., a destination) or to a moving target (e.g., a leading vehicle). The trajectory tracking controller 14 may control the N-trailer vehicle to follow the nominal trajectory 26 as well as account for tracking errors, modeling errors, and/or disturbances. The navigation system 16 may identify, locate, and/or track the target and may detect the operating environment of the N-trailer vehicle. The navigation system 16 may also provide, to the trajectory tracking controller 14, measurements 28 of a state of the N-trailer vehicle based on vehicle output data 30 received from the vehicle model 18. Measurements 28 can be used for autonomous vehicle control. [0053] In some embodiments, the N-trailer vehicle may be a 1-trailer vehicle consisting of a vehicle that is hingedly connected to a single trailer. In some embodiments, the N-trailer vehicle may have two or more trailers, where a first trailer is hingedly connected to the vehicle and all
Atty Docket No. OU-24009WO subsequent trailers are hingedly connected to a preceding trailer in a sequential or daisy-chain like manner. [0054] Fig. 2 is a schematic view of the mission trajectory planner 12 which includes a path planner 32 and a path-trajectory converter 34. Based on the mission objective data 22 and the situational awareness data 24, the path-planner 32 may generate a feasible, collision-free path 36 that connects the starting point to the destination. The path-trajectory converter 34 may receive data defining the path 36 and may assign an appropriate velocity profile along the path, thereby converting the path into the nominal trajectory 26. Path planning may be used to solve one or more autonomous vehicle parking problems. [0055] One or more embodiments described herein may use the mission trajectory planner 12 (e.g., path planner 32 and/or path-trajectory converter 34) for autonomous vehicle parking scenarios, where the mission trajectory planner 12 carries out features which provide improvements over conventional systems. [0056] In some embodiments, the mission trajectory planner 12 (e.g., using the path planner 32) may use a modified four-phase path planning technique to generate a virtual path for a trailer that is part of a 1-trailer vehicle. For example, the path planner 32 may determine a virtual path for the vehicle by using kinematic parameters to relate movement of the trailer (e.g., when moving along the virtual path) to movement of the vehicle. For example, assume a virtual space contains virtual objects such as a vehicle, a trailer hingedly connected to the vehicle, a parking space, an obstacle corner, and a target line in a drive aisle adjacent to the parking space. Further assume the vehicle and trailer (which collectively form a 1-trailer vehicle) are parked in the parking space in an initial pose. In phase one of the four-phase path planning technique, the trailer may be moved from the initial parked position to a ready position from which an obstacle corner can be avoided. In phase two, the trailer may be steered to a target line from the ready position. A reverse path may be determined by moving (steering) the trailer from the parking space to the target line in the drive aisle. In phase three, a forward path may be generated by steering the trailer from the initial pose to the target line. In phase four, the two paths may be connected along the target line to define a virtual path for the trailer. Notably, the four-phase path planning technique described in U.S. Pat. No.11,904,938 is modified such that the path of the trailer is determined, rather than the path
Atty Docket No. OU-24009WO of the vehicle. In the case of an N-trailer vehicle with two or more trailers, the path of the final trailer (e.g., the trailer furthest from the vehicle) is determined using the modified four-phase path planning technique. [0057] Continuing with the example above, the path planner 32 may determine a virtual path for the vehicle by using kinematic parameters to relate movement of the trailer to movement of the vehicle. The virtual path for the vehicle may be defined between an initial vehicle pose and a final vehicle pose. First, the path planner 32 identifies the initial vehicle pose in the parking space. While the trailer is moving between positions along the first virtual path, the path planner 32 may periodically determine kinematic parameters, such as an orientation ^^ of the vehicle and a hitch angle ^^ representing a difference between the orientation ^^ of the trailer and the orientation ^^ of the vehicle. The path planner 32 may also periodically determine a position (^^, ^^) of the vehicle based on a position (^^, ^^) of the trailer, the orientation ^^ of the trailer, and a length ^^ from a hitch point of the vehicle to an axle of the trailer.
allows the path planner 32 to determine the final vehicle pose which is defined as a final orientation ^^ and a final position (^^, ^^) of the vehicle (e.g., at a time during which the trailer is in a final trailer pose). [0058] Notably, instead of finding a path with enough width to accommodate the vehicle or computing the free-space, embodiments of the invention may pad obstacles with a safer buffer zone for collision avoidance, thereby reducing the feasible path to a single line. Advantageously, this feature may improve the computational efficiency of the system. In some embodiments, a feasible path may be generated using Line-Of-Sight (LOS) Pure Pursuit Guidance, which may produce a feasible path segments that are natural and smooth. [0059] Continuing with the example above, the virtual path of the trailer and the virtual path of the vehicle can be then used by the path-trajectory converter 34 to assign a proper velocity profile for the overall path determined by the path planner 32. In some embodiments, the path- trajectory converter 34 may include a velocity profile assignment converter and an optimal path- to-trajectory converter. In the velocity profile assignment approach, a velocity profile in a prespecified shape (e.g., a trapezoidal shape) is assigned on each path segment. In the optimal path-to-trajectory conversion algorithm, the dynamics of the vehicle may be characterized as a 2nd-order integrator, and the path-to-trajectory conversion problem converted to a free ending time
Atty Docket No. OU-24009WO optimal control problem. [0060] Continuing with the example, once the virtual path for the N-trailer vehicle is computed, and a velocity profile is assigned, one or more control signals may be generated that cause a physical N-trailer vehicle to move in a physical space using each respective virtual path. A more detailed description of these embodiments is provided further herein. [0061] One or more embodiments described herein refer to one or more of the following use cases: (1) parallel parking a vehicle along a road; (2) back-in perpendicular parking in an outdoor parking space or in a parking garage; and (3) pulling out of a parking space. That is to say, the one or more path planning techniques described herein may be applied to each respective use case. [0062] In the discussion that follows, coordinate frames are established, and a kinematic model is defined. For example, Fig.3 shows a 1-trailer vehicle 38 that includes a vehicle 40 and a trailer 42. The following coordinate frames may be used to describe the kinematics of the 1-trailer vehicle 38. The coordinate frames may include a ground-fixed North-East-Down (NED) navigation frame (n-frame), a body-carried NED frame (e.g., an ^^^ frame) for the vehicle 40, a body-carried NED frame (e.g., an ^^^ frame) for the trailer 42, a body-fixed frame (e.g., a ^^ frame) for the vehicle 40, and a body-fixed frame (e.g., a ^^ frame) for the trailer 42. [0063] The n-frame, which is shown along the outermost portions of Fig.3, may have an origin ^^ located at a fixed point of interest (e.g., on the ground). The variables ^^, ^^, and ^^ are the Cartesian axes, with ^^ pointing to the north, ^^ pointing to the east, and ^^ pointing down. The n-frame may be treated as a flat and inertial frame. [0064] The ^^^ frame, which is shown near the back side of the vehicle 40, may be parallel to the n-frame and may have an origin ^^^^ located at the midpoint of the rear axle of the vehicle 40. The ^^^ frame, which is shown near the back side of the trailer 42, may also be parallel to the n- frame and may have an origin ^^^^ located at the midpoint of the rear axle of the trailer 42. [0065] The ^^ frame, which is also shown near the back side of the vehicle 40, may have an origin ^^^ that coincides with ^^^^, with ^^^ along a longitude of the vehicle 40 pointing forward, ^^^ pointing to the driver’s right side of the vehicle 40, and ^^^ pointing down. The ^^ frame,
Atty Docket No. OU-24009WO which is also shown near the back side of the trailer 42, may have an origin ^^^ overlapped with ^^^^ , with ^^^ along the longitude of the vehicle 40 pointing forward, ^^^ pointing to the right side of the vehicle 40, and ^^^ pointing down. [0066] Referring still to Fig. 3, the expression (^^, ^^) denotes the Cartesian coordinates of the midpoint of the rear axle of the vehicle 40 in the n-frame. The expression (^^, ^^) denotes the Cartesian coordinates of the midpoint of the rear axle of the trailer 42 in the n-frame. The variable ^^ represents a yaw angle of the ^^^-axis in the ^^^-frame with respect to the ^^^^-axis. The variable ^^ represents a yaw angle of the ^^^-axis in the ^^^-frame with respect to the ^^^^-axis.
The variable δ represents the steering angle of 40. The variables ^^ and ^^ represent the longitudinal velocities of the vehicle 40 and the trailer 42, respectively. The variables ^^ and ^^ represent the yaw rates of the vehicle 40 and the trailer 42, respectively. The variable ^^ represents the wheel base of the vehicle 40. The variable ^^ represents the distance between the midpoint of the axle of the vehicle 40 and the hitching point. [0067] The trailer 42 may be hitched to a midpoint of the rear axle of the vehicle 40. The variable ^^^ represents the front overhang of the vehicle 40. The variable ^^^ represents the rear overhang of the trailer 42. The variable b represents the half width of the vehicle 40. The variable ^^ (shown as a curved arrow at the hitch point) shows the hitch angle. The dashed line in Fig.3 identifies the longitudinal centerline of the 1-trailer vehicle 38. [0068] In some embodiments, an assumption is made that the two wheels on each axis are treated as a single wheel. For example, the equation ^^ = ^ ^ - ^ ^ represents the hitch angle between the vehicle 40 and the trailer 42. A coordinate vector that characterizes the configuration of the 1-trailer vehicle 38 may be taken as ξ = [^ ^ ^, ^^, ^^, ^^] , where the variables ^^, ^^ are the position coordinates of the vehicle 40,
variable ^^ is the orientation of the vehicle 40, and the variable ^^ is the hitch angle. The vehicle 40 is assumed to move at low speed on level ground while parking. It is also assumed that the vehicle is under one or more nonholonomic constraints. It is also assumed that the steering angle δ is bounded by | δ | <= δ^ !. Given these assumptions, the kinematic model of the 1-trailer vehicle 38 is given using the following Equations for a driftless system:
Atty Docket No. OU-24009WO ^"^ = cos ^^ * ^^ (1) ^^" = sin ^^ * ^^ (2) ^˙^ = ^^ (3) ^˙ $%& ' ^ = ^^ - ( )( ^^ (4) [0069] In
because there is no explicit drift term, meaning the motion is controlled by the input variables ^^ and ^^. In Equations (1) and (2), the variables ^"^ and ^^" represent time derivatives of ^^ and ^^, respectively, where ^^ and ^^ represent the position of the rear axle of the vehicle 40 in a global (inertial) frame. The variables ^^" and ^^" represent how fast a position of the vehicle 40 changes in the respective x- and y- directions. That is to say, the variables ^^" and ^"^ represent the velocity of the vehicle 40 in the respective x- and y- direction. The variable ^^ represents the yaw angle (orientation) of the vehicle 40. As described above, the inputs to the system are the variables ^^ and ^^, which represent the longitudinal velocity and yaw rate of the vehicle 40, respectively. Each Equation describes how these variables evolve over time. That is to say, Equations (1) and (2) describe the motion of the vehicle 40 in a global coordinate system. The velocity ^^ determines how fast the vehicle 40 moves. The yaw angle ^^ represents the direction in which the vehicle 40 moves. [0070] In Equation (3), the variable ^˙^ represents a time derivative of the yaw angle ^^ of the vehicle 40. While the yaw angle ^^ represents an instantaneous orientation of the vehicle 40, the time derivative ^˙^ of the yaw angle ^^ represents a rate of change of ^^ over time. That is to say, the time derivative ^˙^ represents the angular velocity of the vehicle 40. The yaw angle ^^ changes at a rate ^^ which is the rotational velocity of the vehicle 40. This equation directly links the steering control of the vehicle 40 to its angular motion. [0071] Equation (4) provides a time derivative ^˙^ of the hitch angle ^^. While the hitch angle ^^ is indicative of how much the trailer 42 rotates relative to the vehicle 40, the time derivative ^˙^ of the hitch angle ^^ is indicative of the rate of change of the hitch angle ^^ over time The variable ^^ represents the rotational velocity of the vehicle 40. When the vehicle 40 rotates, the
Atty Docket No. OU-24009WO trailer 42 is affected, causing ^˙^ to change. The remainder of the expression, i.e., (- *+^ ' ( )( ^^), accounts for the alignment of the trailer 42. If the trailer 42 is misaligned (e.g., if ^˙^
does not equal zero), it will naturally try to align with the dimensions of the vehicle 40 over
with a rate influenced by the velocity ^^ of the vehicle and the length ^^ which a distance from a hitch point of the vehicle 40 to an axle of the trailer 42. If the velocity ^^ of the vehicle 40 is high, the trailer 42 has less time to realign, so ^˙^ changes more gradually. If the velocity ^^ of the vehicle 40 is low, the trailer 42 has to realign, leading to faster hitch
angle adjustments. If the length ^^ is large, the takes longer to respond to steering changes (e.g., due to a slower angle variation). If the length ^^ is small, the trailer 42 reacts
more quickly to movements of the vehicle 40. [0072] Additional context regarding the application of these kinematic equations to a physical environment is provided. The trailer 42 may have a delay when following the vehicle 40. If the vehicle 40 turns, the trailer 42 does not instantly follow, but instead, gradually follows the path of the vehicle 40 due to the hitch constraint. If the hitch angle ^˙^ grows too large, (e.g., rapid backward movements), the trailer 42 can “jackknife” and
uncontrollable. The trailer 42 cannot move sideways and instead, must follow a nonholonomic motion, meaning the trailer 42 follows a restricted path dictated by its geometry. The system is driftless, meaning there is no natural movement unless inputs are provided. The system does not include acceleration dynamics, rather, the system describes how position and orientation change instantaneously with velocity. [0073] One or more examples and/or embodiments described herein may rely on the following assumptions: (1) the parking space is on the right side of the driver; (2) the origin ^^ of the n- frame is located in the parking space and taken as the final position of the trailer, (3) the drive aisle is parallel to the ^^-axis, and (4) there are restrictions that the vehicle body cannot cross in ^^ and ^^ directions. For the general cases where the assumptions (1), (2), and (3) are violated, the question can be put in the same form by an inertial frame transformation. [0074] The four-phase path planning technique presented in U.S. Pat. No. 11,904,938 is incorporated herein by reference, and notably, is extended to for use cases involving autonomous parking of N-trailer vehicles. The path planning technique is applicable to both parallel and
Atty Docket No. OU-24009WO perpendicular parking scenarios with jackknife prevention. The technique has a low computational cost for real-time implementation. The resulting path must be smooth and have a small number of cusps (i.e., sharp changes in direction). In some embodiments, a path planning technique is described herein which, for a 1-trailer vehicle, a parking space, and restrictions on ^^ and ^^, the path planning technique may be used to generate a feasible, collision-free path that connects a starting pose ,* = (^^*, ^^*, ^-*, ^^*) and a final pose ,^ = (^^^, ^^^, ^-^, ^^^). Using the assumptions made herein, the final pose is taken as ,^ = (0,0,. / ,0) for and ,^ =
(0,0,0,0) for perpendicular parking. That is to say, in the parallel parking scenario, the vehicle 40 faces upward (^- - . /, or 90°) (i.e., the final heading of the vehicle 40 is along the positive ^^-axis in the coordinate system), the trailer 42 is aligned with the vehicle 40 (^^ = 0), and the vehicle 40 is positioned at (0,0) meaning it is fully parked. In the perpendicular parking scenario, the vehicle 40 faces straight ahead (^- = 0), the trailer 42 is perfectly aligned with the vehicle 40 (^^ = 0), and the vehicle 40 reaches (0, 0), meaning it is fully parked in a perpendicular spot. Notably, while one or more embodiments described herein apply the path planning technique to a 1-trailer vehicle, one or more other embodiments described further herein apply a similar path planning technique to N-trailer vehicles, where N >= 2. [0075] In some embodiments, a path planning technique may be implemented to determine the path for a 1-trailer vehicle, such as the 1-trailer vehicle 38. For example, assume a virtual space is defined that contains virtual objects including a vehicle (e.g., vehicle 40), a trailer (e.g., trailer 42) hingedly connected to the vehicle, a parking space, an obstacle corner, and a target line in a drive aisle adjacent to the parking space. The modified four-phase path planning technique may be executed to determine a feasible, collision-free virtual path of the trailer 42, where the trailer 42 has dimensions equal to that of the corresponding physical trailer which is located in the physical parking environment. In phase one, a ready position ,^ is determined from which the trailer 42 avoids an obstacle corner when moving along a path between the ready position ,^ and the target line. The trailer 42 may be moved between a first trailer pose in the parking space to a second trailer pose in the ready position. This ensures the trailer 42 avoids obstacles and tight corners. In phase two, the trailer 42 is moved between the second trailer pose and a third trailer pose on the target line. Next, a reverse path (e.g., sometimes referred to as a first path segment) is
Atty Docket No. OU-24009WO determined by tracing the trailer 42 as the trailer 42 is moved between the first trailer pose, the second trailer pose, and the third trailer pose. The reverse path can be used later for forward motion. In phase three, the trailer 42 may be moved between the first trailer pose and a fourth trailer pose on the target line. Next, a new forward path (e.g., sometimes referred to as a second path segment) is determined by tracing the trailer 42 as the trailer 2 is moved between the fourth trailer pose and a final trailer pose. In phase four, forward and reverse paths are connected along the target line to ensure a smooth transition between maneuvers. [0076] Next, a virtual path of the vehicle 40 is determined by using kinematic parameters to relate movement of the trailer 42 (e.g., when moving along the first virtual path) to movement of the vehicle 40. A detailed description for determining the virtual path of the vehicle 40 is provided further herein. [0077] The switching logic in path planning may be selected so that the entire 1-trailer vehicle 38 remains in the available space. Switching logic is a control mechanism that determines when and how a vehicle should transition between different movement phases. In this case, switching between forward and reverse motion or different steering angles may prevent the trailer from colliding with obstacles or exceeding the parking space. [0078] Before determining the virtual pose of the vehicle 40, an overview of the kinematic model for the trailer 42 is provided using the following Equations: ^˙^ = cos ^^ * ^^ (5)
= [0079] Equations (5)-(7) describe the motion of the trailer 42 by using the rear axle of the trailer 42 as a virtual steering axle. In Equation (5), the variables ^˙^ and ^˙^ are time derivatives of the variables ^^ and ^^. While the variables ^^ and ^^ represent the Cartesian coordinates of the rear axle of the trailer 42 in a global coordinate frame, the variables ^˙^ and ^˙^ represent how fast a position of the trailer 42 changes in the respective x- and y- directions. That is to say, the
Atty Docket No. OU-24009WO variables ^˙^ and ^˙^ represent the velocity of the trailer 42 in the respective x- and y- directions. The trailer 42 moves in a direction of its yaw angle ^^, and velocity components of the trailer 42 are defined using Equations (5) and (6), where Equation (5) represents velocity in the x-direction and Equation (6) represents velocity in the y-direction. In Equation (7), the variable ^˙^ represents a time derivative of the yaw angle ^^ of the trailer 42. While the yaw angle ^^ represents an instantaneous orientation of the trailer 42, the time derivative ^˙^ of the yaw ^^ represents
a rate of change of ^^ over time. That is to say, the time derivative ^˙^ represents an angular velocity of the trailer 42. [0080] To simplify the path planning problem, a longitudinal velocity ^^ of the vehicle 40 is normalized as 1^, where | 1^ | = 1. This means that the path of the 42 is parameterized by
distance traveled. After normalization, the kinematic Equations for the trailer 42 can be written as: ^˙^ = cos ^^ * 1^ (8) ^˙^ = sin ^^ * 1^ (9) ^˙^ = ^^1^ (10) [0081] In Equations (8) and (9), the variables ^˙^ and ^˙^ are time derivatives of the variables ^^ and ^^. The variables ^˙^ and ^˙^ represent how fast a position of the trailer 42 changes in the respective x- and y- directions. That is to say, the variables ^˙^ and ^˙^ represent the velocity of the trailer 42 in the respective x- and y- directions. The variable ^^ represents the yaw angle of the trailer 42. The variable 1^, which is found in each of Equations (8), (9), and (10), represents a normalized velocity (set to a value of one for simplification purposes). In Equation (10), the variable ^˙^ represents a time derivative of the yaw angle of the trailer 42. The variable ^^ represents the curvature of the path of the vehicle 40. [0082] For a 1-trailer vehicle, such as 1-trailer vehicle 38, longitudinal velocities and yaw rates between the vehicle 40 and the trailer 42 can be shown using the following Equations: 1^ = cos ^^ * ^^ (11)
Atty Docket No. OU-24009WO ^^ = ^^ 1^ = ^ )( sin ^^ * ^^ (12) [0083] Equations
rate ^^ of the trailer 42 are influenced by the velocity ^^ and hitch angle ^^ of the vehicle 40. For example, in Equation (11),
the variable 1^ represents the normalized velocity of the 42 along a longitudinal axis. The variable ^^ velocity of the vehicle 40. The variable ^^ is the hitch angle, e.g., the difference in yaw vehicle 40 and trailer 42. Equation shows that velocity of the trailer 42 is
always a fraction of the velocity of the vehicle 40, on the hitch angle ^^. That is to say, the hitch angle ^^ determines how much the forward velocity of the 40 contributes to
forward motion of the trailer 42. When ^^ = 0, i.e., when the trailer 42 is aligned with the vehicle 40, then 1^ = ^^, meaning the trailer 42 moves at the same speed as the vehicle 40. When | ^^ | increases, the trailer 42 moves slightly slower than the vehicle 40 because a portion of the
of the vehicle 40 is used for rotation rather than for forward movement. [0084] In Equation (12), the variable ^^ refers to the yaw rate of the trailer 42, i.e., the rate at which the trailer 42 rotates. The variable ^^ represents the curvature of the path of the vehicle 40. The variable ^^ represents a hitch length, i.e., a distance from a hitch point to an axle of the trailer 42. Equation (12) shows that yaw rate ^^ of the trailer 42 depends on the hitch angle ^^ and the velocity ^^ of the vehicle 40. When
0, i.e., when the trailer 42 is aligned with
vehicle 40, then sin ^^ = 0, meaning the trailer
does not rotate, but rather, simply follows the vehicle 40 in a line. When | ^^ | is large, the trailer 42 rotates more because of the offset between headings of the
and the trailer 42. In summary, Equation (11) determines how much the velocity of the vehicle 40 contributes to movement of the trailer 42. Equation (12) determines how much the trailer 42 rotates based on movement of the vehicle 40. [0085] The hitch angle ^^ and the velocity ^^ can be expressed in terms of 1^ and ^^ using the following Equations:
^^ = arctan (^^^^) (13)
Atty Docket No. OU-24009WO [0086] Equations (13) and (14) allow motion of the trailer 42 to be related to control inputs of the vehicle 40. That is to say, while Equations (11) and (12) relate how a given input of the vehicle 40 affects the trailer 42, whereas Equations (13) and (14) determine how the vehicle 40 must move to generate a given trajectory of the trailer 42. [0087] A virtual pose of the vehicle 40 can be determined using Equations (8), (9), and (10), and Equations (13) and (14), which can be represented as: ^^ = ^^ + ^^ (15) ^^ = ^^ + ^^ cos ^^ (16) [0088] Equations
(e.g., a position and an orientation) of the vehicle 40 based on the virtual path or trajectory of the trailer 42. In Equation (15), the orientation of the vehicle 40 is defined as the sum of the orientation of the trailer 42 (^^) and the hitch angle ^^. In Equation (16), the x-axis position of the vehicle 40 is determined by shifting from the x-axis position of the trailer 42 by a distance ^^ along the direction of ^^. Similarly, in Equation (17), the y-axis position of the vehicle 40 is determined by
a distance ^^ along the direction of ^^. [0089] Thus, Equations (15)-(17) determine the pose (e.g., the position and orientation) of the vehicle 40 using a known (e.g., previously computed) position and orientation of the trailer 42. This computation can be made due to the differential flatness of the standard N-trailer, as is described in P. Rouchon, M. Fliess, J. Levine, and P. Martin, “Flatness, motion planning and trailer systems,” in Decision and Control, 1993., Proceedings of the 32nd IEEE Conference. IEEE, 1993, pp.2700-2705. For an N-trailer vehicle, the position of the midpoint of the rear axle of the last trailer is a flat output, i.e., determines the entire state for the system. For a 1-trailer, the following Equations may be used to convert trajectory data for the trailer 42 into yaw and yaw rate which is necessary for computing the motion of the vehicle 40. ^^ = arctan (5˙( !˙() (18)
Atty Docket No. OU-24009WO ^^ = ^˙^ = !˙(5˙˙(^!˙˙(5˙( !˙6 6 ( 75˙( (19) [0090] Equation
42 using its velocity components. The ratio 5˙( !˙( represents the slope of the movement direction of the trailer 42. The arctangent function converts this slope into an angle. Equation (19) may be used to determine a yaw rate ^^ of the trailer 42. The numerator ^˙^^˙˙^ − ^˙˙^^˙^ is the determinant of the velocity and acceleration vectors. The denominator ^˙/ / ^ + ^˙^ normalizes the result based on speed. As described above, Equations (18) and (19) convert trajectory data of the trailer 42 into yaw angle and yaw rate which are used for computing motion of the vehicle 40. [0091] In some embodiments, Equations (13)-(19) may be used together to determine a pose of the vehicle 40. The pose may be determined as a vector-valued function of ^^, ^^ and a finite number of their derivatives. In this case, Equations (13) and (14) may be to determine the
hitch angle ^^ and the velocity ^- of the vehicle 40 based on curvature of the trailer 42. Equations (15)-(17) may be used to determine a pose of the vehicle 40 from a pose of the trailer 42. Equations (18) and (19) may be used to determine a yaw and a yaw rate of the trailer 42 from the velocity of the trailer 42. This allows the pose of the vehicle 40 to be determined as a function of the trajectory of the trailer 42. [0092] To provide a specific example, assume a parallel parking maneuver where the trailer 42 follows a smooth curved path as it reverses into a parking space. The vehicle 40 has to adjust its movement accordingly to guide the trailer 42 while avoiding the jackknifing phenomenon. Using the embodiments described above, a virtual path of the trailer 42 is computed, a yaw and yaw rate of the trailer 42 is determined, a hitch angle and velocity of the vehicle 40 is determined, and a pose of the vehicle 40 is determined. [0093] Due to limitations relating to maneuverability, it is often difficult for a trailer vehicle to adjust its pose by driving back and forth in the parking space. Hence, for 1-trailer parallel parking use cases, an assumption is made that the parking space is wide enough for the 1-trailer to drive in without changing the driving direction. Thus, a corresponding assumption is also made that the parking space is wide enough for a virtual parked trailer to be moved from the parking
Atty Docket No. OU-24009WO space without changing driving direction. [0094] In some embodiments, such as that shown in Fig.4, a 1-trailer vehicle, such as 1-trailer vehicle 38, may be moved from a parallel parking space. In this embodiment, the variable ^+^* represents the instantaneous center of rotation for the 1-trailer vehicle 38. That is to say, ^+^* is a key pivot point around which both the vehicle 40 and trailer 42 turn. The variable 9^ represents the turning radius of the vehicle 40 as measured from ^+^*. The variable 9^ represents the turning radius of the trailer 42 as measured from ^+^*. The variable 9^: represents the turning radius of the right front corner of the vehicle 40 and which is also referred to as a bounding line radius. The variable ^;^< represents the distance between the path of the trailer 42 and the bounding line which is the farthest point of the motion of the vehicle 40 from ^+^*. The variable =^ represents the length of the parking space. The variable >?-^ represents the distance from ^+^* to the obstacle corner. The variable 9?-^ represents the radius of the safety buffer zone around the obstacle corner. It is noted that ^+^*, 9^, 9^, 9^:, >?-^, and ^;^< are functions of the hitch angle ^^. That is to say, each of the turning radii depend on the hitch angle ^^, meaning that as the
angle ^^ increases or decreases, the radii also changes. [0095] Fig.4 shows that the 1-trailer vehicle 38 may be moved from the parking space with a maximum joint angle ^^,^ !. The solid arc represents the path of the trailer 42 while being moved out of the parking space. The dashed arc represents the bounding line, which is the path traced by the right front corner of the vehicle 40. As such, 9^: is also the radius of the bounding line, meaning that 9^: represents the widest part of the motion of the vehicle 40. [0096] Notably, if there exists a path along which the 1-trailer vehicle 38 can be moved from the parking space without changing driving direction, the parking space must be long enough to ensure that >?-^ (^^,^ !) > 9^: (^^,^ !). That is to say, the 1-trailer vehicle 38 must have sufficient space to be moved without the bounding line hitting the obstacle. This means that the distance from ^+^* to the obstacle corner >?-^ must be greater than the bounding line radius 9^:. Otherwise, the bounding line may intersect with the obstacle boundary and the 1-trailer vehicle 38 would collide with the corner if it continued driving forward. [0097] Referring again to Fig.4, the variable ^+^+ represents the initial distance from a rear of
Atty Docket No. OU-24009WO the vehicle 40 to the obstacle corner along the ^^-axis. The length of the parking space must be larger than ^+^+. To ensure this, a lower bound of the length =^ may be determined using Equation (20): = > ^ > @29 ^ + ^/ + 2^ ^/ + / ^ +^+ ^,^+^ ^ B ^ 9^,^+^ + ^^^ + ^^ (20) [0098]
radius of the vehicle 40. The variable b represents a geometric offset parameter related to vehicle dimensions (e.g., wheel base or axle offset) and which affects turning dynamics. The variable ^^ represents the length of the vehicle 40 from front to rear axle. This impacts the required clearance when maneuvering. The variable ^^^ represents a distance parameter related to the hitch position or an additional safety buffer and accounts for motion constraints of the trailer 42. The variable ^^ represents a hitch length, e.g., a distance from a hitch of the trailer 42 to the rear axle. This affects how the trailer 42 follows the vehicle 40. When 9^,^+^ = )( $%& '(,CDE, then >?-^ (^^,^ !) > 9^: (^^,^ !). That is to say, Expression (20) ensures that the parking space is long enough to allow the 1-trailer vehicle 38 to be moved safely out of the parking space without hitting any obstacles. [0099] In some embodiments, such as in the context of perpendicular parking, the 1-trailer vehicle 38 can be moved from the parking space by driving along a straight line. In some embodiments, such as in the context of parallel parking, a sufficiently large safety buffer zone is drawn around the obstacle corner so that both the vehicle 40 and the trailer 42 can pass the corner without collision. The size of the safety buffer zone may be selected so that (1) the entire vehicle body can avoid the corner (e.g., between phase one and phase two of the four phase path planning technique), and so that (2) the 1-trailer vehicle 38 can be moved from the parking space without collision. By treating the trailer 42 as a car-like vehicle, condition (1) is satisfied if the conditions in Equation (20) are satisfied. [00100] Another way to determine if condition (1) is satisfied is by using the following: 2RG%&2RHIJ - R/ HIJ > 2RG%&b - ^/ + ^/, where e is the distance between qJ and the point where the line-of-sight is tangent to the safety zone, b is the half-width of the vehicle, and RG%&
Atty Docket No. OU-24009WO satisfies RG%& > max {b, e} as the minimum turning radius of the vehicle. If the vehicle continues to drive with δ > 0 and if the avoidance corner is guaranteed to be avoided by the entire vehicle body, then the radius of the safety buffer zone satisiies 2RG%&2RHIJ - R/ HIJ > 2RG%&b - ^/ + ^/. [00101] An estimated size of the safety buffer zone that satisfies condition (1) may be determined using Equation (21): 9?-^ > B9/ + + ^/ ^ + ^/ ^ + 9/ + + b (21) [00102] In
, the safety buffer zone around the obstacle corner. This is the space required to guarantee safe clearance. The variable 9+ / represents a lower bound on the turning radius of the trailer 42 along the retrieval path (i.e., 9^ > 9^). The variable ^^ represents the length parameter. The variable ^^ represents the length from the hitch point to the axle of the trailer. The variable b represents an additional buffer term to account for extra clearance for safety. The variable ^;^< represents the distance between the bounding line of the vehicle movement and the obstacle corner. To avoid collisions, the safety buffer zone radius 9?-^ must be greater than ^;^<. For example, if 9?-^ is too small, the bounding line will enter the obstacle zone, causing a collision. If 9?-^ is greater than ^;^<, the 1-trailer vehicle 38 can safely clear the corner. While not expressed in this application, a proof of Equation (21) can be determined to conclude that 9?-^ > 9^: - 9^ = ^;^<, which ensures avoidance safety. [00103] In some embodiments, a jackknife prevention constraint may be implemented as part of the four-phase path planning technique. Jackknifing occurs when the hitch angle ^^ becomes too large, causing the trailer 42 to fold sharply relative to the vehicle 40.
a jackknife prevention constraint ensures that the hitch angle ^^ does not exceed a safe limit, preventing the trailer 42 from folding too sharply relative to
vehicle 40. As will be explained below, this approach ensures that the curvature ^^ of the trailer 42 remains within safe limits, thereby keeping ^^ below a maximum allowable value ^^,^ !. [00104] In some embodiments, to implement the jackknife prevention constraint, Equations (13) and (14) may first be re-written as the following Equation:
Atty Docket No. OU-24009WO | ^^ | = | arctan (^^, ^^) | <= ^^,^ ! < . / (22) [00105] In Equation
of the virtual path of the trailer 42, ^^ is the hitch arctan (^^, ^^) expresses the hitch angle in terms of curvature and
hitch length, ^^,^ ! is the allowable hitch angle that prevents the trailer 42 from folding . too far relative to the vehicle 40. The upper bound / ensures that the trailer 42 never becomes perpendicular to the vehicle 40, thereby preventing the jackknifing phenomenon from occurring. Overall, in Equation (22), the absolute value of the hitch angle ^^ must be below a maximum threshold hitch angle ^^,^ !. This ensures that the trailer 42 folds excessively relative to the
vehicle 40. Equation (22) can be re-written as Equation (23): | ^^ | <= jk& '(,CDE )( (23) [00106] In
, path of the trailer 42, tan ^^,^ ! expresses the
hitch angle in trigonometric form, and ^^ adjusts the curvature constraint based on the physical connection between the vehicle 40 and the trailer 42. Equation (23) ensures that the trailer 42 follows a path with a safe, smooth curvature, avoiding abrupt changes that could lead to jackknifing. By limiting ^^, the hitch angle ^^ never reaches potentially dangerous values. Equation (22) defines ^^ in terms of
(^^, ^^), which requires computationally expensive trigonometric
By enforcing the
in terms of ^^, as is done using Equation (23), the need for frequent trigonometric evaluations in a real-time system is eliminated, thereby conserving computing/processing resources. [00107] In some embodiments, Equation (23) may be implemented as part of the four-phase path planning technique to ensure all computed paths satisfy the jackknife prevention constraint. In phase one, the trailer 42 is moved from the parking space to a ready position ,^. When the trailer 42 is moved, the curvature ^^ is monitored to ensure that ^^ does not exceed the threshold (e.g., jk& '(,CDE ). If ^^ exceeds
threshold, adjustments are made to keep the trailer 42 from
angles that could lead to jackknifing. If the initial retrieval path is too tight (high curvature), a wider arc must be generated to satisfy Equation (23). The ready position is determined such that it allows enough space for gradual steering changes, while keeping ^^
Atty Docket No. OU-24009WO within the safe range. [00108] In phase two, the trailer 42 is moved backward so that it aligns with the target line. As the trailer 42 is adjusted to align with the target line, ^^ is continuously monitored. If | ^^ | exceeds the threshold, the maneuver is adjusted. If the curvature is too high, the alignment maneuver must be executed over a longer distance to reduce curvature. If ^^ is close to ^^,^ !, minor forward movements may be introduced to reset the hitch angle motion.
[00109] In phase three, a forward path is generated from the initial pose to the target line pose. In phase four, the paths are connected smoothly to finalize the path of the trailer 42. In phases three and four, ^^ is also continuously monitored and compared against the threshold. At each phase, the path planner ensures that the computed curvature of the trailer 42 satisfies the constraints of Equation (23). If a planned path violates the curvature limit, the system adjusts the trajectory to remain within safe limits. This prevents jackknifing while allowing flexible maneuverability. [00110] For path planning, the steering angle δ has to remain within physical limits. The steering constraint indirectly limits the rate of curvature change ^^ . , preventing unstable or infeasible motion. The mathematical steps involve substituting key relationships to express δ in terms of curvature of the trailer 42 and its rate of change. A discussion of how to implement the steering constraint follows. [00111] In some embodiments, a steering angle constraint may be implemented. For example, a steering angle δ of the vehicle 42 may be determined using a kinematic equation that expresses the steering angle δ in terms of a curvature ^^ and a steering constraint that limits a speed at which the trailer is capable of changing
In this embodiment, the steering angle δ of the vehicle 40 may be adjusted if the steering angle δ is not within a permissible range defined by an upper bound steering angle maximum l^ ! and a lower bound steering angle minimum l^+^. This embodiment is described in further detail below using Equations (24)-(28). [00112] In some embodiments, the steering angle δ may be determined using the following Equation: $% . δ = arctan m & '( + '(
Atty Docket No. OU-24009WO [00113] Equation (24) establishes a relationship between δ and ^^ and ^^ .. By substituting Equations (13) and (14) for ^^ and ^^ in Equation (24), the Equation is formed:
) δ =
( m )( sin ^ )m )( . ^ + (^7 )6 p q6 s/6 p ) ^^ 1^) (25) [00114] In Equation (25), the
40 needed to achieve the curvature change of the trailer 42. That is to say, Equation (25) expresses δ in terms of the curvature ^^ of the virtual path of the trailer 42 (e.g., how sharply the trailer 42 is turning at a given instant) and its rate of change ^^ .. A large ^^ means the trailer 42 is making a tight turn (small radius) and a low ^^ means trailer 42 is making a wide turn (large radius). The first
) term in the arctan m
( )( sin ^^) represents the contribution of the hitch angle ^^ to the ) ) steering angle. The second term in the arctangent function ( m ( (^7 )6 p q6 ()s/6 ^^ .1^) includes the variable ^^ . , which represents the rate of change of
turning of the trailer 42 is changing). The variable 1^ represents the velocity of the trailer 42. The denominator ((1 + ^/ + ^/ ^)u//) ensures the curvature change is smoothed out. The steering angle δ is now dependent on the rate of change of curvature ^^ .. This introduces a constraint in that steering limits will not limit how fast ^^ can change. If ^^ is too large, the required steering angle would exceed physical limits. Under the
on steering angle constraint, Equation (25) yields a constraint on the rate of change of the curvature ^^ which can be represented using the following Equations: l^+^(1^, ^^, ^^) <= ^^ . <= l^ ! (1^, ^^, ^^) (26)
[00115] In Equation (26), the rate of change of curvature ^^ . is bounded by l^+^ and l^ !. These limits ensure that the trailer 42 does not turn too aggressively, preventing extreme steering angles. Referring to Equations (27) and (28), the variable l^+^ represents the lower bound on the rate of change of the curvature ^^ . And the variable l^ ! represents the upper bound on the rate
Atty Docket No. OU-24009WO of change of the curvature ^^ .. The variable tan δ^ ! represents the maximum possible steering ^ angle allowed by the vehicle 40. The expression )m)( tan δ^ ! relates the maximum steering rate to the rate of change of the curvature ^^ . of the trailer 42. The expression ^ )6 sin ^^ * 1^ accounts
( for the effect of the hitch angle on limits. The expression / u//
+ ^+ r ensures
smoothness and prevents abrupt changes to curvature. [00116] The larger the steering angle δ^ !, the wider the limits on ^^ .. The hitch angle ^^ also affects how quickly the curvature can change. If ^^ . exceeds these limits, the steering
angle δ would be infeasible, leading to an Thus, the equations described above
directly limit how quickly the trailer 42 can change direction. The four-phase path planning technique is implemented to adhere to these limits when computing turning paths. This ensures smooth, feasible motion and prevents steering saturation, e.g., a situation where the vehicle 40 cannot steer fast enough to match the curvature change of the trailer 42. [00117] In order to consider the maximum steering angle constraint in the design, a magnitude limiter described by Equation (26) is applied to ^+ .. The output of the limiter is integrated as the actual curvature employed in path planning. [00118] One or more embodiments described herein provide examples to demonstrate the path planning technique applied to parallel parking and back-in perpendicular parking scenarios. For example, assume the dimensional constraint parameters for a cargo truck is ^^ = 4.5 meters (m), ^^^ = 1.8 m, ^^^ = 1 m, b = 1.25m, and ^^ = 9 m. The mechanical constraint parameters are chosen as ^^ = 1 radian (rad) (e.g., approximately 57.3 degrees) and δ^ ! = 0.4 rad (e.g., approximately degrees). The dimensions of the parking space and the restrictions on ^^ and ^^ in the different parking scenarios are given in Table I below:
[00119] In Table I, the variable =v refers to the length of a first (e.g., long) side of the parking
Atty Docket No. OU-24009WO space. The variable =* refers to the length of a second (e.g., short) side of the parking space. The width of the drive aisle may be 3.66 m. [00120] For a simulation of a parallel parking scenario, the radius of the avoidance circle may be determined using Equation (21), with one meter extra safety tolerance added to the lower bound radius. The middle line of the drive aisle may be selected as the target line. The ready position ,^ is selected as the position where the geometric center of the trailer 42 is on the avoidance circle. The parameters of the switching controllers are chosen as ^^ = 0.9, ^/ = 4.5, ^u = 25, and ^w = 1.6. Equations described herein may be applied to that the conditions in each respective
equation are satisfied for all scenarios. The initial and values for δ and ^^ are 0 rad in all cases. The simulation may be conducted using a high-level programming and numerical
computing environment such as Matrix Laboratory (MATLAB) or using another similar programming language and numerical computing environment. In one embodiment, the computing time for the simulation is 90 milliseconds (ms). [00121] Example results from parking simulations are shown in connection with Figs.6A and 6B, Figs. 7A and 7B, Figs. 8A and 9B, and Figs 9A and 9B. The results for a parallel parking simulation on the side of a drive aisle are shown in Figs.6A and 6B and in Figs.7A and 7B. In Fig.6A, a collision-free path is successfully planned for a 1-trailer vehicle. For example, Fig.6A shows a top down view of a 1-trailer vehicle, where motion over time is shown while the 1-trailer vehicle performs a parallel parking maneuver. In Fig.6A, a series of boxes 56 show poses of the 1-trailer vehicle along a path. A shadowed area represents obstacles that the vehicle 38 must avoid. Two dotted lines 50, 52 (shown as being perpendicular to each other) indicate restrictions on ^^ and ^^, respectively. The dashed line represents the target line 58. The solid line 54 represents the line that separates two drive aisles. The solid black lines 60 represent an area containing axles of the 1-trailer vehicle. Fig.6B shows a representation of the path taken by the 1-trailer vehicle of Fig.6A. The path of the trailer is shown using reference number 62. The path of the vehicle is shown using reference number 64. [00122] Fig.7A shows a graph plotting data points 66 that represent a hitch angle relative to a distance traveled over the course of the parallel parking maneuver. Fig.7B shows a graph plotting data points 67 that represent a steering angle relative to a distance traveled over the course of the
Atty Docket No. OU-24009WO parallel parking maneuver. [00123] Figs. 8A and 8B and Figs. 9A and 9B show simulation results for a perpendicular parking scenario. Perpendicular parking for trucks requires larger space than in parallel parking and often take place in parking garages. Figs.8A and 8B show that a collision-free path is planned for reverse perpendicular parking in a parking garage. Fig. 8A is a top down view of a 1-trailer vehicle, where motion over time is shown while the 1-trailer vehicle performs a perpendicular parking maneuver. Fig.8B shows a representation of the path taken by the 1-trailer vehicle of Fig. 8A. Figs. 9A and 9B show that the constraints on ^^ and δ are satisfied along the path. Fig.9A shows a graph plotting data points 68 that a hitch angle relative to a distance traveled
over the course of the perpendicular parking maneuver. Fig.9B shows a graph plotting data points 69 that represent a steering angle relative to a distance traveled over the course of the perpendicular parking maneuver. [00124] In some embodiments, a path planning technique is implemented for a parking scenario involving an N-trailer vehicle. To illustrate the application of the path planning technique for an N-trailer vehicle (e.g., a vehicle with more than one trailer), an N-trailer vehicle 44 is shown in Fig.5. N-trailer vehicle 44 may include a vehicle 46 and a set of trailers (shown as a first trailer 48-1, …, a penultimate trailer 48^^^, and a final trailer 48^). Similar to the 1-trailer vehicle 38, a North-East-Down (NED) navigation frame (n-frame) and NED body-carried frames (^^+-frames, where 0 <= i <= N) for the vehicle 46 and for the trailers 48. The n-frame may have an origin ^^ located at a fixed point of interest (e.g., on the ground). The variables ^^, ^^, and ^^ are the Cartesian axes, with ^^ pointing to the north, ^^ pointing to the east, and ^^ pointing down. The n-frame may be treated as a flat and inertial frame. [00125] The ^^+ frames may be parallel to the n-frame and may have an origin ^^^+ located at the midpoints of the rear axle of each respective body (e.g., the vehicle 46 and each respective trailer 48-1, …, 48-N), where ^^^^ refers to the origin point for the vehicle 46. The ^^ frame may have an origin ^^^ that coincides with ^^^^, with ^^^ along a longitude of the vehicle 40 pointing forward, ^^^ pointing to the driver’s right side of the vehicle 46, and ^^^ pointing down. The ^+ frame may have an origin ^^+ that coincides with ^^^^ , with ^^+ along the longitude of a given trailer pointing forward, ^^+ pointing to the right side of the given trailer, and ^^+ pointing down.
Atty Docket No. OU-24009WO [00126] The distance from the midpoint of the axle of the i-th trailer to the hitching point of the same trailer may be represented using the variable ^+. A coordinate vector that characterizes the configuration of the N-trailer vehicle is taken as may be taken as ξ = [^ ^ ^, ^^, ^^, … , ^^] . In this case, the variables ^^, ^^ represent the Cartesian coordinates of the midpoint of the axle of the vehicle 46 in the n-frame, the variable ^+ represents the yaw angle of the ^^+-axis in the ^^+-frame with respect to the ^^^+-axis. The inputs to the system are ^^ and ^^, which represent the longitudinal velocity and yaw rate of the vehicle 46, respectively. [00127] The variables ^+, ^+ represent the Cartesian coordinates of the midpoint of the axle of the i-th trailer. The expression ^+ = ^+^^ – ^+, where i >= 1, represents the joint angle between two consecutive bodies (e.g., between 46 and trailer 48-1, between trailer 48-1 and trailer
48-2, etc.). It is noted that ^+ serves as the steering angle for the i-th trailer when the trailer is treated as an independent vehicle. The wheel base of the vehicle 46 is represented using the variable ^^. The distance between the midpoint of the rear axle of the i-th trailer and the hitching point may be represented using the variable ^+. The front overhang of the vehicle 46 may be represented using the variable ^^^. The rear overhang of the last trailer may be represented using the variable ^^^. The half-
of the vehicle may be represented using the variable b. [00128] The kinematic model for the N-trailer vehicle 44 may be provided using Equations (1)- (3) and Equations: ^˙^ = - *+^ '( ) ^^ + ^^ (29)
[00129] - are kinematic equations for the vehicle 46. Equation (29) describes how the hitch angle ^˙^ between the vehicle *+^ ' 46 and the first trailer 48-1 changes. The expression - ( )( ^^ accounts for the effect of the geometry of the trailer 48-1. If ^^ is a non-zero value, then it will influence turning behavior of the first trailer 48-1. The variable ^^ refers to the yaw rate of the vehicle 46. The rotation of the vehicle 46 directly affects ^^. Thus, ^^ evolves based on the motion of the vehicle 46 and the
Atty Docket No. OU-24009WO hitch constraints. [00130] Equation (30) can be implemented to determine the hitch angle for the i-th trailer (e.g., where i >= 2). The product term ∏+ |^ }^ ^ accounts for the effect of all previous hitch angles ^| in the chain. The more trailers that are attached, the greater the effect of each hitch angle on the next adjoining trailer. The term jk& 'p~( )p~( depends on the hitch angle of the previous trailer (e.g., ^+^^). The term $%& 'p )p depends on the hitch angle of the current trailer (e.g., ^+). Together, these terms are used to how much the hitch angle of the current trailer changes based on the motion of the trailer. The variable ^^ represents the velocity of the vehicle 46. The rate of change of ^+ is directly proportional to the velocity ^^ of the vehicle 46. [00131] As can be seen from Equations (29) and (30), the hitch angle of each trailer depends on all previous trailers, creating a cascading effect. The more trailers that are in the system, the more complex the motion dynamics become. The hitch angle of the first trailer 48-1 is only affected by the vehicle 46, but all other trailers accumulate effects from trailers that are further up in the chain (e.g., closer to vehicle 46). [00132] In some embodiments, the N-trailer vehicle 44 may be moved from a parallel parking space. The parking environment and the assumptions made are the same as those stated in connection with the parking scenarios for the 1-trailer vehicle 38. Variables are provided for this example. The variable ^+^* represents the instantaneous center of rotation for the N-trailer vehicle 44. That is to say, ^+^* is a key pivot point around which the vehicle 40 and trailers 42 turn. The variable 9+ (0 <= i <= N) and 9^: may be defined as the turning radii of the i-th trailer (where i = 0 for the vehicle 46) and the right front corner of the vehicle 46, respectively. The variable ^;^< is the distance between the last trailer’s path and the bounding line which is the path of the farthest point of the vehicle 46 from ^+^*. The variable =^ refers to the length of a first (long) side of the parking space. The variable >?-^ refers to the distance from ^+^* to the obstacle corner. The variable 9?-^ refers to the radius of the safety buffer zone around the obstacle corner. As such, for a given N-trailer, a parking space, and restrictions on ^^ and ^^, a four phase path planning technique may be applied to generate a feasible, collision-free path that connects the start pose ,* = (^^*, ^^*, ^^*, ^^*, …, ^^*) and a final pose ,^ = (^^^, ^^^, ^^^, ^^^, …, ^^^).
Atty Docket No. OU-24009WO [00133] To solve the path planning problem, a cascading technique may be extended from the four-phase path planning technique to generate paths for the trailers under the jackknife prevention constraints. Then, paths may be generated for the bodies in an order starting from the last trailer and ending with the vehicle 46. The path may be computed using the following steps: in each sampling time, (1) a new path point is planned for the last trailer by using the four-phase path planning technique, and (2) for i = N – 1 to 0: a new path point of the i-th trailer (where i = 0 is for the vehicle 46) is computed using the 1-trailer technique described herein according to the path point of the (i + 1)-th trailer by treating the i-th trailer as a vehicle 46. Then, path points of the i- th and the (i + 1) trailer go through a jackknife prevention limiter. The path points for the (i + 1)- th to the N-th trailers are modified to satisfy the jackknife prevention constraint if it is violated. [00134] Given the longitudinal velocity ^+ and the curvature ^+ for the i-th trailer path, ^+ and ^+^^ can be computed using a modified version of Equations (13) and (14), where the variable ^+ the variable ^^ and where the variable ^+^^ replaces the variable ^^. Thus, the path of the (i – 1)-th trailer can be computed from the path of the i-th trailer by using a modified version of Equations (15), (16), and (17). Similar to the modification made to Equations (13) and (14), modified Equations (15)-(17) simply replace the sub notation 1 with the sub notation of i or 1-i, while utilizing the same overall equation. Furthermore, the jackknife prevention constraint may be defined using a similarly modified version of Equations (22) and (23). In modified Equation (22), ^+ is the curvature of the i-th trailer path and ^+,^ !, which bounds the absolute value of ^+, is selected to prevent jackknifing. ^+ can be expressed as a smooth function of ^^ and of its first N-i derivatives with respect to the traveled distance of the N-trailer. Thus, the jackknife prevention constraint can be transformed to a constraint on ^^ and its first N-i derivatives. The transformed constraint can be implemented into step two of the cascading technique. [00135] Similar to the 1-trailer vehicle, if there exists a path along which the vehicle can be moved from the parking space without changing driving direction, the parking space must be long enough to ensure that >?-^ (^^,^ !, …, ^^,^ !) > 9^: (^^,^ !, …, ^^,^ !). To determine the lower bound of the length of the parking space, Equation (31), which is a modified version of Equation (20), determines the lower bound in a manner that accounts for N-trailer vehicles.
Atty Docket No. OU-24009WO ^ > @29 ^ + ^/ + 2^ ^/ + 9/ ∑^ +^+ ^,^+^ ^ B ^ ^,^+^ + ^^^ + +}^ ^+ (31) [00136]
a parking space, where ^+^+ is the initial distance from the rear of the vehicle to the obstacle corner along the ^^-axis. If ^+^+ satisfies Equation (31), where 9^,^+^ = max {l^, …, l^} in which: l+ = B )6 p ^ ^6 'p,CDE 4 ∑+ |}^ ^/ | (32) [00137]
!). [00138] In some embodiments, a safety buffer radius is selected for the N-trailer vehicle 44. Assume a sufficiently long parking space and a retrieval path. Assume that the turning radius 9^ of the final trailer is bounded by 9^ > 9∗ ^ , for some constant 9∗ ^ > 0 along the path. If a radius 9?-^ of the safety buffer zone satisfies Equation (36) which is shown below, then 9?-^ > ^;^<, i.e., the vehicle is retrieved without collision. 9 / ^ = B9∗ ^ 4 ∑^ / ∗ ?- ^+ - 9^ + ^ (33)
[00139] 44 as parameters (e.g., ^+, ^^^, ^^^, ^, and ^+,^ !) and can be adapted to vehicles with different dimensions by setting the parameters. As such, these embodiments provide improvements to scalability when compared with neural network based data driven approaches which are computationally expensive and difficult to implement in real-time. According to the computing steps, the time complexity of the cascading technique is the same as that of the four-phase technique, which is O (N). Thus, when compared against existing approaches, the techniques described herein are extremely computationally efficient. [00140] In some embodiments, a hitching offset is provided. For an N-trailer with a hitching offset, ^+ ≠ 0, I = 1, …, N., the velocities and yaw rates between the two bodies are related to each other Equation (34):
Atty Docket No. OU-24009WO ^ cos ^+ ^+ sin ^+ ^^ +^^ +^^ ^ = ^ ^ sin ^ )p + − cos ^+^ ^ ^ ^+ + ^ (34)
^. + = - ^ )p sin ^+ * ^+^^ + (1 + ^p~( )p cos ^+) ^+^^ = ^ ^p~( sin ^+ * ^+ – ( )p ^p~( cos ^+) ^+ (35)
forward motion with ^+^^ > 0 but input-out unstable for reverse motion with ^+^^ < 0. By contrast, by
treating ^+ as a control the kinematics of ^+ are input-output stable for a reverse motion path with ^+^^ < 0. This observation indicates that, for general N-trailer vehicles, a bounded path may not be obtained by using the previous cascade path planning algorithm directly. For general N- trailer systems in forward motion, the kinematic inverse from the position ^^, ^^ of the midpoint of the rear axle of the last trailer to the vehicle longitudinal velocity ^^ and yaw rate ^^ is unstable. Therefore, there are inherent difficulties extending the previous cascade-like path planning algorithm to general N-trailers directly. [00143] As described herein, the four-phase path planning technique developed for a standard vehicle has been extended to 1-trailer and N-trailer vehicles. The four-phase path planning technique is used to generate a path for the trailer. By utilizing the flatness property of standard trailers, the path of the vehicle can be determined based on the trailer path. Limiters are introduced to the design to account for the jackknife prevention constraint and the steering angle constraint. Then, the technique is further extended to a cascade technique for N-trailer parking scenarios. The effectiveness of techniques described herein is verified by simulations where the results illustrate that the technique is able to generate a feasible, collision-free path in various parking scenarios. [00144] Referring now to Fig. 10, embodiments of the invention described above may be implemented using one or more computer devices or systems, such as exemplary computer system 70. The computer system 70 may include a processor 72, a memory 74, an input/output (I/O) interface 76, and a Human Machine Interface (HMI) 78. The computer system 70 may be operatively coupled to one or more external resources 80 via the I/O interface 76 or a network 82. The computer may be configured to support real-time communication channels to control actuators
Atty Docket No. OU-24009WO 94 and navigation sensors 92. [00145] The processor 72 may include one or more devices selected from microprocessors, micro-controllers, digital signal processors, microcomputers, central processing units, field programmable gate arrays, programmable logic devices, state machines, logic circuits, analog circuits, digital circuits, or any other devices that manipulate signals (analog or digital) based on operational instructions that are stored in the memory 74. Memory 74 may include a single memory device or a plurality of memory devices including but not limited to read-only memory (ROM), random access memory (RAM), volatile memory, non-volatile memory, static random access memory (SRAM), dynamic random access memory (DRAM), flash memory, cache memory, or any other device capable of storing information. The memory 74 may also include data storage devices such as a hard drive, optical drive, tape drive, non-volatile solid state device, or any other device capable of storing information. A database 84 may reside in memory 74 and may be used to collect and organize data used by the various systems and modules described herein. [00146] Processor 72 may operate under the control of an operating system 86 that resides in memory 74. The operating system 86 may manage computer resources so that computer program code embodied as one or more computer software applications, such as application 88 residing in memory 74, may have instructions executed by the processor 72. The operating system 86 may be a real-time operating system that process data as it comes in with little or no buffer delays, and that has well-defined and consistent time constraints. In an alternative embodiment, the processor 72 may execute the application 88 directly, in which case the operating system 86 may be omitted. One or more data structures 90 may also reside in memory 74, and may be used by the processor 72, operating system 86, or application 88 to store or manipulate data. [00147] The I/O interface 76 may provide a machine interface that operatively couples the processor 72 to other devices and systems, such as the network 82 or external resource 80. The application 88 may thereby work cooperatively with the network 82 or external resource 80 by communicating via the I/O interface 76 to provide the various features, functions, or modules comprising embodiments of the invention. The application 88 may also have program code that is executed by one or more external resources 80 or otherwise rely on functions or signals
Atty Docket No. OU-24009WO provided by other system or network components external to the computer system 70. Indeed, given the nearly endless hardware and software configurations possible, persons having ordinary skill in the art will understand that embodiments of the invention may include applications that are located externally to the computer system 70, distributed among multiple computers or other external resources 80, or provided by computing resources (hardware and software) that are provided as a service over the network 82, such as a cloud computing service. [00148] The HMI 78 may be operatively coupled to the processor 72 of the computer system 70 to allow a user to interact directly with the computer system 70. The HMI 78 may include video or alphanumeric displays, a touch screen, a speaker, and any other suitable audio and visual indicators capable of providing information to the user. The HMI 78 may also include input devices and controls such as an alphanumeric keyboard, a pointing device, keypads, pushbuttons, control knobs, microphones, etc., capable of accepting commands or input from the user and transmitting the entered input to the processor 72. [00149] In general, the routines executed to implement the embodiments of the invention, whether implemented as part of an operating system or a specific application, component, program, object, module or sequence of instructions, or a subset thereof, may be referred to herein as “computer program code,” or simply “program code.” Program code typically comprises computer-readable instructions that are resident at various times in various memory and storage devices in a computer and that, when read and executed by one or more processors in a computer, cause that computer to perform the operations necessary to execute operations or elements embodying the various aspects of the embodiments of the invention. Non-transitory computer- readable program instructions for carrying out operations of the embodiments of the invention may be, for example, assembly language, source code, or object code written in any combination of one or more programming languages. [00150] Various program code described herein may be identified based upon the application within which it is implemented in specific embodiments of the invention. However, it should be appreciated that any particular program nomenclature which follows is used merely for convenience, and thus the invention should not be limited to use solely in any specific application identified or implied by such nomenclature. Furthermore, given the generally endless number of
Atty Docket No. OU-24009WO manners in which computer programs may be organized into routines, procedures, methods, modules, objects, and the like, as well as the various manners in which program functionality may be allocated among various software layers that are resident within a typical computer (e.g., operating systems, libraries, API's, applications, applets, etc.), it should be appreciated that the embodiments of the invention are not limited to the specific organization and allocation of program functionality described herein. [00151] The program code embodied in any of the applications/modules described herein is capable of being individually or collectively distributed as a computer program product in a variety of different forms. In particular, the program code may be distributed using a non-transitory computer-readable storage medium having program instructions thereon for causing a processor to carry out aspects of the embodiments of the invention. [00152] Computer-readable storage media, which is inherently non-transitory, may include volatile and non-volatile, and removable and non-removable tangible media implemented in any method or technology for storage of data, such as computer-readable instructions, data structures, program modules, or other data. Computer-readable storage media may further include RAM, ROM, erasable programmable read-only memory (EPROM), electrically erasable programmable read-only memory (EEPROM), flash memory or other solid state memory technology, portable compact disc read-only memory (CD-ROM), or other optical storage, magnetic cassettes, magnetic tape, magnetic disk storage or other magnetic storage devices, or any other medium that can be used to store data and which can be read by a computer. A computer-readable storage medium should not be construed as transitory signals per se (e.g., radio waves or other propagating electromagnetic waves, electromagnetic waves propagating through a transmission media such as a waveguide, or electrical signals transmitted through a wire). Computer-readable program instructions may be downloaded to a computer, another type of programmable data processing apparatus, or another device from a computer-readable storage medium or to an external computer or external storage device via a network. [00153] Computer-readable program instructions stored in a computer-readable medium may be used to direct a computer, other types of programmable data processing apparatuses, or other devices to function in a particular manner, such that the instructions stored in the computer-
Atty Docket No. OU-24009WO readable medium produce an article of manufacture including instructions that implement the functions, acts, or operations specified in the flowcharts, sequence diagrams, or block diagrams. The computer program instructions may be provided to one or more processors of a general purpose computer, a special purpose computer, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the one or more processors, cause a series of computations to be performed to implement the functions, acts, or operations specified in the flowcharts, sequence diagrams, or block diagrams. [00154] The flowcharts and block diagrams depicted in the figures illustrate the architecture, functionality, or operation of possible implementations of systems, methods, or computer program products according to various embodiments of the invention. In this regard, each block in the flowchart or block diagrams may represent a module, segment, or portion of instructions, which comprises one or more executable instructions for implementing the specified logical function or functions. [00155] In certain alternative embodiments, the functions, acts, or operations specified in the flowcharts, sequence diagrams, or block diagrams may be re-ordered, processed serially, or processed concurrently consistent with embodiments of the invention. Moreover, any of the flowcharts, sequence diagrams, or block diagrams may include more or fewer blocks than those illustrated consistent with embodiments of the invention. It should also be understood that each block of the block diagrams or flowcharts, or any combination of blocks in the block diagrams or flowcharts, may be implemented by a special purpose hardware-based system configured to perform the specified functions or acts, or carried out by a combination of special purpose hardware and computer instructions. [00156] Fig.11 is a flowchart of an example process 96 for computing a virtual path of a virtual 1-trailer vehicle and for using the virtual path to move a corresponding physical 1-trailer vehicle in a physical environment. As shown by block 98, process 96 includes defining virtual objects including a vehicle, a trailer connected to the vehicle, a parking space, and a target line adjacent to the parking space. For example, mission trajectory planner 12 (e.g., using path planner 32) may define virtual objects including a vehicle, a trailer connected to the vehicle, a parking space, and a target line adjacent to the parking space.
Atty Docket No. OU-24009WO [00157] As shown by block 100, example process 96 further includes determining a first virtual path for the trailer between a first trailer pose in the parking space and a final trailer pose outside of the parking space. For example, mission trajectory planner 12 (e.g., using path planner 32) may determine a first virtual path for the trailer between a first trailer pose in the parking space and a final trailer pose outside of the parking space. [00158] As shown by block 102, example process 96 further includes determining a second virtual path for the vehicle between a first vehicle pose in the parking space and a final vehicle pose outside of the parking space by using kinematic parameters to relate movement of the trailer when moving along the first virtual path to movement of the vehicle. For example, mission trajectory planner 12 (e.g., using path planner 32) may determine a second virtual path for the vehicle between a first vehicle pose in the parking space and a final vehicle pose outside of the parking space by using kinematic parameters to relate movement of the trailer when moving along the first virtual path to movement of the vehicle. [00159] As shown by block 104, example process 96 further includes generating one or more control signals that cause a physical vehicle connected to a physical trailer to move in a physical space, where movement of the physical vehicle corresponds to the second virtual path and movement of the physical trailer corresponds to the first virtual path. For example, trajectory controller 14 may generate one or more control signals that cause a physical vehicle connected to a physical trailer to move in a physical space, where movement of the physical vehicle corresponds to the second virtual path and movement of the physical trailer corresponds to the first virtual path. [00160] While the present invention has been illustrated by a description of various embodiments and while these embodiments have been described in considerable detail, it is not the intention of the applicants to restrict or in any way limit the scope of the appended claims to such detail. Additional advantages and modifications will readily appear to those skilled in the art. The invention in its broader aspects is therefore not limited to the specific details, representative apparatus and method, and illustrative example shown and described. Accordingly, departures may be made from such details without departing from the spirit or scope of applicant's general inventive concept.
Atty Docket No. OU-24009WO [00161] It will be apparent that systems and/or methods described herein, may be implemented in different forms of hardware, firmware, and/or a combination of hardware and software. The actual specialized control hardware or software code used to implement these systems and/or methods is not limiting of the embodiments. Thus, the operation and behavior of the systems and/or methods were described herein without reference to specific software code – it being understood that software and hardware can be used to implement the systems and/or methods based on the description herein. [00162] Some embodiments are described herein in connection with thresholds. As used herein, satisfying a threshold may, depending on the context, refer to a value being greater than the threshold, more than the threshold, higher than the threshold, greater than or equal to the threshold, less than the threshold, fewer than the threshold, lower than the threshold, less than or equal to the threshold, equal to the threshold, etc., depending on the context. [00163] No element, act, or instruction used herein should be construed as critical or essential unless explicitly described as such. Also, as used herein, the articles “a” and “an” are intended to include one or more items and may be used interchangeably with “one or more.” Furthermore, as used herein, the term “set” is intended to include one or more items (e.g., related items, unrelated items, a combination of related and unrelated items, etc.), and may be used interchangeably with “one or more.” Where only one item is intended, the phrase “only one” or similar language is used. Also, as used herein, the terms “has,” “have,” “having,” or the like are intended to be open- ended terms. Further, the phrase “based on” is intended to mean “based, at least in part, on” unless explicitly stated otherwise.