This invention relates generally to path planning of motion of the vehicles, and more particularly to path planning systems and methods for automatically controlling a motion of a vehicle between different states.
Several control systems employed by vehicles, either autonomous vehicles or vehicles executing in autonomous-driving mode, predict future, safe motions, or paths, of the vehicle, both in order to avoid obstacles, such as other vehicles or pedestrians, but also to optimize some criteria associated to the operation of the vehicle. The vehicle is controlled along the path that is a part of the object trajectory or, more precisely, the projection of the trajectory onto the two-dimensional plane of movement. To say it more figuratively, the path is the line connecting all geometrical coordinates of the moving vehicle.
A fundamental objective of path planning is to construct a trajectory connecting two states, e.g., an initial state and a target state, of a moving object, such as autonomous or semi-autonomous vehicles executing in autonomous-driving mode, and to control the motion of the vehicles along the trajectory. A “state” as used herein is meant to be a vector including the geometrical coordinates of the vehicle in the plane and an orientation of the vehicle at these geometrical coordinates. The target state can either be a fixed location, a moving location, a velocity vector, a region, or a combination thereof. The surroundings, such as road edges, pedestrians, and other vehicles, are sensed by the sensors of the vehicle and/or are at least partially known by a priori given information.
A path connecting an initial state and a target state is normally made up of several elementary paths (or EPT for short). In some situations, the path planning solely works with elementary paths of the same or different kinds. To that end, the path of a moving body is normally made up of a plurality of elementary paths that share the same junction points (abbreviated as JP). Thus, the junction points are special points of the path because each of them belongs to two adjoining elementary paths (EPT).
One special characteristic demanded of the paths is that states do not change erratically along the entire course or at the junction points. This is to say that the transition from one elementary path to an adjacent elementary path must be ensured to be continuous at the junction points of the elementary paths as well as both its first derivative (tangent) and its second derivative (curvature). The path is continuous at junction points when the transition from one elementary path to the adjacent elementary path can be performed by changing a steering and velocity of the vehicle in a linear or continuous manner.
For example, pioneering work by Dubins and Reeds and Shepp investigated patterns of a shortest path between two states of the vehicle. The shortest path, termed as Reeds-Shepp's (RS) path, is a sequential composition of line segments and/or tangential circular arcs of a minimum turning radius. The RS path can be computed in a very efficient way, but the curvature along the RS path involving multiple segments is discontinuous. Such discontinuity of the curvature is undesirable in practice, since a vehicle has to stop and perform stationary steering, leading to unnecessary time delay and extra wearing of tires.
Some methods overcome discontinuity of the RS paths by introducing other segments, in addition to the line and circular segments. For example, the path planning can combine various types of elementary paths such as splines, polynomials, trigonometric functions or clothoids. The aforementioned elementary paths are mainly distinguished by their mathematical description. A feature that all elementary paths have in common is that they can be combined with straight and/or circular lines to form a continuous curvature path.
Some conventional methods prefer to base its path planning on clothoids because they are closely related to the rules of movement of steered motor vehicles. A clothoid is a special type of plane curves, which has its curvature, denoted by c, changing in a linear manner along the path. Clothoids have the added benefit of the length of the path being minimized when an initial state is connected with a terminal state.
For example, the teaching of T. Fraichard and A. Scheuer, “From Reeds and Shepp's to continuous curvature paths,” introduces a concept of a clothoid turn (abbreviated as CC turn) formed by two clothoids connected by a circular arc, and teaches μ-tangency conditions to ensure continuous curvature transitions among CC turns and straight lines. However, a continuous path typically includes a sequence of multiple CC turns and lines, and it can be time consuming to find the parameters of the optimal sequence forming the continuous path.
For example, method described in U.S. 2016/03133735 first determines an RS path and tries to find a continuous curvature path which stays as close as possible to the RS path. This method computes the continuous curvature path by solving optimization problems with nonlinear constraints. However, the solution to this method requires iterative process, which can be computationally prohibitive for a number of practical applications. Also, the resultant optimization problem of that method is non-convex. A feasible solution to the non-convex problem is not guaranteed solved by the iterative process, and thus the construction of the continuous curvature path may fail. Hence, this method may not be suitable for a real-time control of the vehicles.
Similarly, the method described in U.S. Pat. No. 8,428,820 improves the computation efficiency, but still entails computationally expensive iterative process to construct the continuous curvature path. Hence, this method also may not be suitable for a real time control of the vehicles.
Accordingly, there is a need to construct a continuous curvature path for controlling a motion of a vehicle from an initial to a target state in a manner suitable for real-time control of the vehicles.
It is an object of some embodiments to provide a path planning system and method to construct a continuous curvature path connecting an initial state to a target state of a vehicle. It is another object of some embodiments to provide such a system and a method that are suitable for real-time control of the vehicles. As used herein, the continuous curvature path is as a sequential composition of multiple elementary paths connected such that the transition from one elementary path to an adjacent elementary path is continuous at the junction points of the elementary paths as well as both its first derivative and second derivatives. Examples of an elementary path include straight line, circular arc, splines, polynomials, trigonometric functions, clothoids, and clothoid turns.
A continuous curvature path connecting an initial and a target state of a vehicle can be formed by different sequences of the elementary paths. The sequential composition of types of elementary paths is referred herein as a pattern of elementary paths or just as a pattern. For example, a line, a spline, and a polynomial form a pattern that includes three specific types of the elementary paths arranged in a specific order. Geometrical parameters, such as dimensions of each elementary path and the mutual arrangement of the elementary paths within a pattern can vary.
Each pattern used by various embodiments represents a continuous curvature path. As used herein, a pattern represents a continuous curvature path if for some initial and target states, there exist geometrical parameters for the types of the elementary paths forming the pattern that result in the continuous curvature path connecting the initial and target states. For example, a straight line and the circular arc cannot form a continuous curvature path, and thus those types cannot be adjacent in a pattern. On the other hand, a clothoid turn and a straight line can form a continuous curvature path if μ-tangency conditions are satisfied, and thus the combinations of the clothoid turn and the straight line can form a pattern.
Some embodiments are based on recognition that a single continuous curvature path can be formed by different patterns of elementary paths. Moreover, for a single pattern, there can be different combinations of geometrical parameters of its elementary paths that can form the required continuous curvature path. Accordingly, the system defining the continuous curvature path is underdetermined. The solution of the underdetermined systems usually entails computationally expensive iterative process and can be undesirable.
Some embodiments are based on a realization that geometrical constraints imposed on a mutual arrangement of elementary paths within a pattern can turn the underdetermined system into a determined one. The determined system has at most one independent solution that can be found analytically. Thus, for the determined systems, the computationally expansive iterative solutions can be avoided to improve the performance of the processor and memory usage of the path planning.
Some embodiments are based on another realization that such an additional geometrical constraint can be determined for an individual pattern off-line. This realization allows to replace a real-time, i.e., online, determination of the optimal sequence of the elementary paths with the off-line determination of the optimal geometrical constraints. The optimal geometrical constraints can vary for different patterns and can be determined such that the analytical solution for the continuous curvature path that satisfies those geometrical constraints is at least close to the solution that would result from the iterative optimization.
For example, some additional geometrical constraints for different patterns are determined from exploitation of symmetry of patterns, analysis of μ-tangency conditions, and comparison of lengths of feasible paths in a specific pattern. For example, one embodiment uses symmetry to derive additional geometric constraints for a driving pattern CCu|CuC. Additionally or alternatively, geometric analysis is exploited to derive additional insights to determine continuous curvature paths analytically in driving patterns:
To that end, some embodiments predetermined a set of analytical functions corresponding to a set of patterns of elementary paths. In response to receiving values of the initial and target states during the online path planning, the embodiments selects an analytical function corresponding to a minimum cost of the continuous curvature path connecting the initial and target states and determines analytically the parameters of that continuous curvature path using the selected analytical function. In such a manner, the undesirable optimization procedure is avoided.
For example, in one embodiment, a memory of a path planning systems stores a pair of functions including an analytical function and a cost function determined for different patterns. In response to receiving values of the initial and target states, the embodiment determines the cost of each pattern using their corresponding cost function, selects the analytical function corresponding to a pattern with minimum cost, and invokes the selected analytical function to produce the continuous curvature path. For example, the continuous curvature path can be represented by a sequence of coordinates of the points connecting the initial to the target states, a sequence of control inputs resulting in the movement of the vehicle according to the continuous path, or both.
Some embodiments are based on recognition that a pattern may not be feasible for all possible combinations of the initial and target states. It is possible that a pattern may not represent a feasible continuous curvature path for the specific values of the initial and target states. In those situations, the analytical function determined for the pattern has no solution for the specific values of the initial and target states. For example, in one embodiment, when the analytical function of a pattern has no solution for the specific values of the initial and target states, the cost of the continuous path for that pattern is infinitely large.
Some embodiments are based on recognition that when the patterns are selected naively, there can be a situation that no pattern represents a feasible continuous curvature path for some values of the initial and target states and the path planning procedure may fail. To avoid this situation, some embodiments determine analytical functions for an exhaustive set of patterns. As used herein, a set of patterns is jointly or collectively exhaustive, if for any values of the initial and target states, there is at least one pattern that represents a feasible continuous path connecting the initial and target states without consideration of the obstacles.
For example, some embodiments are inspired by the possible patterns of Reeds-Shepp (RS) path, which is a sequential composition of line segments and tangential circular arcs of a minimum turning radius. The RS path is the shortest path that connects the initial and the target states and can have up to 48 driving patterns, further categorized as 12 classes. The RS path is a discontinuous curvature path. However, some embodiments are based on recognition that the discontinuous RS path can be transformed into a continuous curvature path by replacing the circular arcs with corresponding clothoid turns. In such a manner, a transformed continuous RS path closely resembles the structure of the RS path and thus the 48 patterns of the transformed RS paths form an exhaustive set of patterns. As used herein, each transformed RS path is formed by a sequential composition of elementary paths selected from a group including a clothoid turn and a straight line. In one embodiment, each transformed RS path includes a clothoid turn. In one embodiment, each transformed RS path includes an elementary path selected from a group consisting of a clothoid turn and a straight line.
Some embodiments are based on recognition that the adjacent elementary paths in the patterns of the transformed RS paths need to satisfy the μ-tangency condition to ensure continuous curvature transitions among CC turns and straight lines. The μ-tangency conditions is [can you define it please]. The μ-tangency condition is a geometrical constraint on mutual arrangement of the adjacent elementary paths. However, such a geometrical constraint fails to form a determined system for all patterns of the transformed RS path. To that end, some embodiments impose additional geometrical constraints on the transformed RS paths and determine the analytical functions having solutions satisfying both the μ-tangency condition and the additional geometrical constraint.
Accordingly, one embodiment discloses a system for controlling a motion of a vehicle. The system includes a memory to store a set of analytical functions corresponding to a set of patterns of elementary paths, where each pattern represents a continuous path and each analytical function is determined for a corresponding pattern to provide an analytical solution for input states of the vehicle defining a continuous path connecting the input states by a sequential compositions of the elementary paths following the corresponding pattern; a path planner to select from the memory, in response to receiving an initial state and a target state of the vehicle, an analytical function corresponding to a minimum cost of the continuous curvature path connecting the initial state with the target state and to analytically determine parameters of the continuous curvature path using the selected analytical function; and a controller to control the motion of the vehicle according to the parameters of the continuous curvature path.
Another embodiment discloses a method for controlling a motion of a vehicle, wherein the method uses a processor coupled to a memory storing a set of analytical functions and a set of cost functions corresponding to a set of patterns of elementary paths, each pattern represents a continuous path, each analytical function is determined for a corresponding pattern to provide an analytical solution for input states of the vehicle defining a continuous path connecting the input states by a sequential compositions of the elementary paths following the corresponding pattern, and each cost function is determined to provide a cost of the corresponding pattern indicative of a cost of the motion of the vehicle according to the continuous path connecting the input states and represented by the corresponding pattern, wherein the processor is coupled with stored instructions implementing the method, wherein the instructions, when executed by the processor carry out at least some steps of the method. The method includes receiving an initial state and a target state of the vehicle; determining the cost of each pattern indicative of the cost of the motion of the vehicle according to a continuous path represented by the corresponding pattern and connecting the initial state with the target state; selecting an analytical function of the pattern having the minimum cost; analytically determining parameters of the continuous path using the selected analytical function; and controlling the motion of the vehicle according to the parameters of the continuous path.
Yet another embodiment discloses a non-transitory computer readable storage medium embodied thereon a program executable by a processor for performing a method, wherein the memory stores a set of analytical functions and a set of cost functions corresponding to a set of patterns of elementary paths, each pattern represents a continuous path, each analytical function is determined for a corresponding pattern to provide an analytical solution for input states of the vehicle defining a continuous path connecting the input states by a sequential compositions of the elementary paths following the corresponding pattern, and each cost function is determined to provide a cost of the corresponding pattern indicative of a cost of the motion of the vehicle according to the continuous path connecting the input states and represented by the corresponding pattern. The method includes receiving an initial state and a target state of the vehicle; determining the cost of each pattern indicative of the cost of the motion of the vehicle according to a continuous path represented by the corresponding pattern and connecting the initial state with the target state; selecting an analytical function of the pattern having the minimum cost; analytically determining parameters of the continuous path using the selected analytical function; and controlling the motion of the vehicle according to the parameters of the continuous path.
according to some embodiments.
according to some embodiments.
according to some embodiments.
according to some embodiments.
according to some embodiments.
according to some embodiments.
where a curvature κ is an extra configuration parameter in addition to (x, y, θ) Control inputs in the vehicle model (1) are u=(v,σ), where ν is a driving velocity of the rear wheels and σ a steering rate. The relationship between ϕ, κ and σ is established as follows:
Assume that both forward and backward motions are allowed for the vehicle and the driving velocity is bounded, whereas the angle of the steering wheel is subject to mechanical constraints, i.e.,
The steering rate σ is also assumed to be bounded, i.e.,
|σ|≤σmax. (3)
According to some embodiments, a configuration or state of the vehicle is defined by q=(x,y,θ,κ).
Note that any path satisfies (1) and the boundary conditions has continuous curvature κ along the path, and thus is a continuous curvature path.
Some embodiments are based on recognition that CCPP can be fulfilled by combining clothoid turns (CTs or CC Turns) and line segments in the similar manner as the RS path. In one embodiment, a clothoid is a curve whose curvature κ varies linearly with respect to its arc length s, i.e., κ(s)=σmaxs, where σmax is the sharpness of the clothoid.
In one embodiment, configurations of a clothoid, defined over 0≤s≤κmax/σmax, can be determined by integrating system (1) along the clothoid arc
where
are the Fresnel cosine and sine integrals, respectively. The clothoid is plotted in
A deflection of the clothoid represents a difference in orientations of qs and q1. For fixed κmax and σmax, the clothoid has important properties
1. a constant deflection δc=κmax2/(2σmax)
2. a constant arc length s=κmax/σmax.
3. a uniquely determined solution q(s).
These properties can be pre-computed and stored in memory for computational efficiency.
An example of such a clothoid turn is given by an arc 111 from qs to qf in
1. a clothoid from qs to q1, with a sharpness σmax;
2. a circular arc of radius κmax−1 and of angle δf−2δc, starting from q1 and ending at q2;
3. a second clothoid starting from q2 with sharpness −σmax and ending at qf.
When δ=0, a clothoid turn reduces to a line segment of length 2RΩ sin μ, where RΩ and μ are defined later. For 0≤δ<2δc, a clothoid turn contains a clothoid of a sharpness σ≤σmax and a symmetric clothoid of a sharpness −σ, where the sharpness σ is given by
and the length of each clothoid is √{square root over (δ/σ)}. Clothoid turns with 2δc+π≤δ<2π correspond to left backward motions, and can be treated similarly. It is clear that a clothoid turn can be parameterized by its deflection δ, and a line segment can be parameterized by its length l.
One concept in simplifying the CCPP from geometric perspectives is CC Circle.
Interestingly, each CC circle has exactly the same center as the circular arc of its respective clothoid turn. For instance, Ω, the center of the CC circle 121 Cl+(qs), coincides with the center of the circular arc which is tangential to the vehicle orientation at q1. Its coordinates are given by
For fixed κmax and σmax, the CC circle radius and μ-angle are given by
where μ is the angle between the vehicle orientation and a tangent vector of the CC circle 121 at qs.
Some embodiments disclose CCPP methods to determine a continuous curvature path between qs and qf, by combining line segments and clothoid turns. In one embodiment, the continuous curvature path is assumed to take same classes and driving patterns of the RS paths listed in
Specifically, this embodiment is based on recognition that the discontinuous RS path can be transformed into a continuous curvature path by replacing the circular arcs with corresponding clothoid turns. In such a manner, a transformed continuous RS path closely resembles the structure of the RS path and thus the 48 patterns of the transformed RS paths form an exhaustive set of patterns. As used herein, each transformed RS path is formed by a sequential composition of elementary paths selected from a group including a clothoid turn and a straight line. In one embodiment, each transformed RS path includes a clothoid turn. In one embodiment, each transformed RS path includes an elementary path selected from a group consisting of a clothoid turn and a straight line.
A continuous curvature path, with a specified driving pattern, between the initial configuration qs and the final configuration qf, can be parameterized by deflections of clothoid turns and lengths of line segments. For instance, assuming a driving pattern L+S+L+, the continuous curvature path begins with a left forward clothoid turn with a deflection δ1, followed by a line with a length l, and another left forward turn with a deflection δ2. For those skilled in the art, given values of parameters δ1,δ2,l and the initial configuration qs, it is straightforward to obtain a solution of (1), denoted by ϕ(qs,t) with its final configuration given by ϕ(qs,δ1,l,δ2). According to the CCPP problem formulation, the CCPP for qs and qf is equivalent to solve for δ*1,δ*2,l*, which satisfy a set of nonlinear equations
ϕ(qs,δ*1,l*,δ*2)=qf. (7)
The set of equations (7) is non-convex in the parameters, and thus is unreliable to solve. Also, it resorts to iterative algorithms and thus is computationally expensive.
In one embodiment, one can re-parameterize a continuous curvature path and perform the CCPP using μ-tangency conditions, which determine combination of clothoid turns and lines. There are three possible combinations in continuous curvature paths fitting the driving patterns in
1. the CC circles 141 and 142 have to be tangential;
2. a distance L(Ω1Ω2) between centers 143 and 144 of the CC circles 141 and 142 is equal to 2RΩ.
1. the CC circles 145 and 146 intersect with each other, and at an intersecting configuration 149, tangent vectors 147 and 148, of the CC circles 145 and 146, respectively, form angles of μ with respect to an orientation 149 of the vehicle at 149;
2. a distance L(Ω1Ω2) between centers 151 and 152 of the CC circles 145 and 146 should be 2RΩ cos μ.
Some embodiments are based on recognition that a single continuous curvature path can be formed by different patterns of elementary paths. Moreover, for a single pattern, there can be different combinations of geometrical parameters of its elementary paths that can form the required continuous curvature path. Accordingly, the system defining the continuous curvature path is underdetermined. The solution of the underdetermined systems usually entails computationally expensive iterative process and can be undesirable.
Some embodiments are based on a realization that geometrical constraints imposed on a mutual arrangement of elementary paths within a pattern can turn the underdetermined system into a determined one. The determined system has at most one independent solution that can be found analytically. Thus, for the determined systems, the computationally expansive iterative solutions can be avoided to improve the performance of the processor and memory usage of the path planning.
Some embodiments are based on another realization that such an additional geometrical constrain can be determined for an individual pattern off-line. This realization allows to replace a real-time, i.e., online, determination of the optimal sequence of the elementary paths with the off-line determination of the optimal geometrical constraints. The optimal geometrical constraints can vary for different patterns and can be determined such that the analytical solution for the continuous curvature path that satisfies those geometrical constraints is at least close to the solution that would result from the iterative optimization.
To that end, some embodiments analytically compute a continuous curvature path and/or its cost for various driving patterns, such as driving patterns in
The memory 230 can also optionally store a set of cost functions 233 corresponding to the set of patterns and the set of analytical functions. The cost functions 233 allows to analytically compute cost of continuous curvature path according the corresponding pattern. Examples of the cost include a length and/or a time of travel along the path following the pattern. The memory 230 can also optionally store the set of patterns 231 itself. Additionally, or alternatively, the set of analytical and cost function are determined for the set of patterns, but the set itself is not stored in the memory.
A path planner 240 is configured to analytically determine 245 a continuous curvature path connecting a pair of input states referred herein as an initial state and a target state. For example, the path planner can be implemented using a processor programmed to select 243 from the memory 230, in response to receiving 241 an initial state and a target state of the vehicle, an analytical function corresponding to a minimum cost of the continuous curvature path connecting the initial state with the target state and to analytically determine 247 parameters of the continuous curvature path using the selected analytical function.
A controller 250 controls the motion of the vehicle according to the parameters of the continuous curvature path. For example, the parameters of the continuous curvature path includes a sequence of coordinates of points on a plane of the motion of the vehicle forming the continuous curvature path. Additionally, or alternatively, the parameters of the continuous curvature path includes a sequence of control inputs, wherein the control of the motion of the vehicle according to the sequence of control inputs transitions the states of the vehicle according to the continuous curvature path.
When the memory stores a set of cost function corresponding to the set of patterns and the set of analytical functions, the path planner 240 can determine the cost of each pattern using the corresponding cost function and can select the analytical function corresponding to the pattern having the minimal cost. Additionally, or alternatively, the path planner can construct an RS path connecting the initial and the target state and select the analytical function corresponding to a pattern of the RS path. In such a manner, the construction and/or storage of the cost function is not necessary.
In one embodiment, the set of patterns is an exhaustive set that guaranties that for any values of the input states, there is at least one pattern that represents a feasible continuous curvature path connecting the input states without consideration of obstacles. For example, in one embodiments, the set of patterns includes 48 different patterns representing transformed Reeds-Shepp (RS) paths, wherein each pattern in the set is formed by replacing a circular arc in a corresponding pattern representing an RS path with a clothoid turn.
For example, in one embodiment, the analytical functions are determined to provide solutions satisfying μ-tangency conditions on a mutual arrangement of adjacent elementary paths forming the continuous curvature path, and wherein at least some of the analytical functions are determined to provide solutions satisfying an additional geometrical constraint on a mutual arrangement of at least one intermediate elementary path with respect to at least one boundary elementary path.
Given qs and qf, this embodiment uses analytical cost formula to compute costs of continuous curvature paths for all patterns in
In some embodiments, a clothoid, satisfying system dynamics (1) for a pair of fixed κmax and σmax, is pre-computed and stored in memory 201, and can be reused online to determine continuous curvature paths analytically.
In one implementation, the analytical function can include four arc generation blocks: LFCT 222 for generation of an arc corresponding to a left forward clothoid turn based on its start configuration and deflection; LBCT 223 for generation an arc corresponding to a left backward clothoid turn based on its start configuration and deflection; RFCT 224 for generation of an arc corresponding to a right forward clothoid turn based on its start configuration and deflection; RBCT 225 for generation of an arc corresponding to a right backward clothoid turn based on its start configuration and deflection; and LINE 226 for generation of a line segment based on its start configuration and length. A continuous curvature path can be constructed by providing start configurations, deflections, or length as inputs to 222, 223, 224, 225, and 226, respectively, in the order specified by the driving pattern. For example, to compute a feasible continuous curvature path in the driving pattern L+S+R+, the analytical function 220 needs inputs: δ1, l, δ2 and qs, representing a deflection of a left forward clothoid turn, a length of forward line segment, and a deflection of a right forward clothoid turn, the initial configuration, respectively. The analytical function first uses LFCT(qs,δ1) 222 to obtain a left forward clothoid turn arc 222 with δ1 and qs as inputs, which ends at q1; then uses LINE(q1,l) 226 to determine a line tangent to the left forward clothoid turn at q1 with inputs l and q1 as its start configuration, which ends at a configuration q2; finally uses RFCT(q2,δ2) 224 to compute a right forward clothoid turn arc which is tangentially connected with the line segment at q2, and reaches the final configuration qf. The continuous curvature path is a union of the left forward clothoid turn, the forward line segment and the right forward clothoid turn: P=LFCT(qs,δ1)∩LINE(q1,l)ψRFCT(q2,δ2). The analytical function is further illustrated by
A collision-free continuous curvature path planner first constructs a graph G, according to the environment map and initial/final configurations 301 and the vehicle kinematical model 303. The graph G includes a node set N and an edge set E, where each node in N corresponds to a collision-free configuration qi, and each edge in E between two nodes qi,qj implies existence of a collision-free continuous curvature path between qi and qj. The construction 302 outputs the graph G if it contains a set of edges connecting the initial configuration qs to the final configuration qf. A collision-free continuous curvature path P between qs and qf is then determined in 304 on the basis of the graph G by using state of the art search algorithms such as A*, D, D*. A reference trajectory enforcing the vehicle following the continuous curvature path is determined in 305, on the basis of a dynamic model of the vehicle 306. The reference trajectory is further exploited to control actuators on the vehicle to control its motion.
In some embodiments, the reference trajectory defines profiles of the vehicle velocity and steer angle over time. In another embodiment, the reference trajectory defines the profile of the vehicle state (x, y, θ) over time. The vehicle dynamic model is fifth order and given by
where a1 the translational acceleration and a2 the steering angular velocity. The vehicle kinematic model 301 is given by (1).
Additionally, or alternatively, the parking spots can be detected using the sensors 503 of the automated parking system. In one embodiment, the motion planning block checks to determine whether the final state is parkable, i.e., there is a feasible path to the parking spot, or not, and notifies the final state selection block 502 the check result. If the final state is not parkable, the target selection block 502 selects another final state for evaluation. In another embodiment, the final state selection block 501 can also evaluate whether the final state is parkable, and only sends a parkable final state to the motion planning block.
If the final state is parkable, then the motion planning 503 initiates a complete motion planning procedure to determine a reference trajectory 541 based on the vehicle models 510, the initial and final states of the vehicle, and the map of the parking space. In one embodiment, the reference trajectory defines profiles of the vehicle velocity and steer angle over time. In another embodiment, the reference trajectory defines the profile of the vehicle state (x, y, θ) over time.
Given the reference trajectory 541, vehicle controllers and actuators 204 determine and exert control commands to enforce the vehicle state track the reference trajectory 541 if the reference trajectory is state profile, or to enforce the vehicle velocity and steer angle track the reference trajectory if the reference trajectory is the vehicle velocity and steer angle profiles. In one embodiment, the control commands could be gas pedal pressure or steering torque. The vehicle controller/actuators may also use signal 543 to determine control commands. The signal 543 can be a measured steering angle, or measured currents of motors moving the steering wheel or the gas pedal.
In some embodiments, the analytical functions are determined to provide solutions satisfying μ-tangency conditions on a mutual arrangement of adjacent elementary paths forming the continuous curvature path. At least some of the analytical functions are determined to provide solutions satisfying an additional geometrical constraint on a mutual arrangement of at least one intermediate elementary path with respect to at least one boundary elementary path. Some embodiments use existence conditions, geometric constraints, validation conditions imposed on the cost function 200 and the analytical function 220 to determine a continuous curvature path connecting qs and qf.
classes can be constructed similarly to the
classes, this invention explicitly presents here existence conditions for 10 classes:
Therefore, the L+S+L+ path that connects q0 and qf includes:
1. A left clothoid turn CT1: since the orientation of 715A of CT1 is identical to 711A, we have the deflection of CT1: δ1=θ;
2. A straight line segment 706A of length
Existence conditions: Such an L+S+L+ path exists if and only if L(Ω1Ω2)≥2RΩ sin μ and θf≥θ are both satisfied. Validation condition requires absolute values of deflections of all clothoid turns are less than π+2δc. A function arclen computes lengths of clothoid turns, with deflections of clothoid turns as its inputs.
The desired canonical path L+S+R+ can therefore be generated with (9).
1. A left clothoid turn CT1 with a deflection δ1=θ+α;
2. A straight line segment 706B of length
L(q1q2)=√{square root over (L2(Ω1Ω2)−(2RΩ cos μ)2)}−2RΩ sin μ;
3. A second left clothoid turn CT2: since the configuration 716B q2 of CT2 shares the same orientation with 715B q1, we conclude that the deflection of CT2 shall be δ2=θf−θ−α.
Existence conditions: Such an L+S+R+ path exists if and only if L(Ω1Ω2)≥2RΩ and both δ1 and δ2 satisfy validation conditions.
The L+R−L+ path suggests that the two left forward clothoid turns be connected by a right and backward clothoid turn, which implies that a CC circle 703C Cr−(q1) must coincide with Cr+(q2). Let Ω3 denote the center of the CC circle 703C, and let α denote an angle between a line 706C Ω1Ω2 and a line 705C Ω1Ω3. The μ-tangency conditions between 701C Cl+(q0) and 703C Cr−(q1) and between 703C Cr−(q1) and 702C Cl−(qf) suggest
L(Ω1Ω3)=L(Ω2Ω3)=2RΩ cos μ. (10)
Applying law of cosines to a triangle Ω1Ω2Ω3 yields
Therefore the coordinate of Ω3 can be given by
Let θ1 denote an angle 713C between a line 707C Ω2Ω3 and the positive x-axis.
The path L+R−L+ includes
1. A left clothoid turn CT1: the μ-tangency between 701C Cl+(q0) and 703C Cr−(q1) asserts that the orientation of 715C q1 is
which is exactly the deflection δ1 of the CT1;
2. A backward and right clothoid turn CT2: since the orientation of 716C q2 is
according to the μ-tangency between 703C Cl−(qf) and 702C Cr−(q1), the deflection of CT2 is δ2=θ1−θ−α−π;
3. A second left clothoid turn CT3 with a deflection
Existence conditions: the triangle Ω1Ω2Ω3 should be valid, i.e.,
0≤L(Ω1Ω2)≤4RΩ cos μ.
Validation conditions are: absolute values of δ1δ2 and δ3 are less than π+2δc.
Based on the μ-tangency between 701D Cl+(q0) and 703D Cr−(q1), and between 703D Cr−(q1) and 702D Cl+(qf), we have
L(Ω1Ω3)=2RΩ cos μ,L(Ω2Ω3)=2RΩ. (13)
Let α denote an angle 712D between a line 705D Ω1Ω2 and a line 706D Ω1Ω3. The law of cosines suggests that, in a triangle Ω1Ω2Ω3,
Coordinates of Ω3 can also be represented by (12), except that the α is computed by (14). Let θ1 denote an angle 713D between a line 707D Ω2Ω3 and the positive x-axis. Thus the path L+R−L− includes
1. A left clothoid turn CT1 with a deflection
2. A backward and right clothoid turn CT2: since the orientation of 716D q2 is
a deflection of CT2 is δ2=θ1−θ−α−μ−π;
3. A backward and left clothoid turn CT3 with a deflection
Existence conditions: the triangle Ω1Ω2Ω3 be valid, i.e.,
2RΩ(1−cos μ)≤L(Ω1Ω2)≤2RΩ(1+cos μ).
Validation conditions are: absolute values of δ1, δ2 and δ3 are less than π+2δc.
Let 715E q1 and 716E q2 be two intermediate configurations along with the path. To determine a center Ω3 of a CC circle 703E Cr−(q1), we apply law of cosines in a triangle Ω1Ω3Ω2 to obtain an angle 712E α:
which yields coordinates of Ω3
Let 713E θ1 denote an angle between a line 707E Ω2Ω3 and the positive x-axis.
The path L++R+L− is the sequential concatenation of
1. A left clothoid turn CT1: the μ-tangency between 701E Cl−(q0) and 703E Cr+(q1) implies that the orientation of 715E q1 is
i.e, the CT1 has a deflection
2. A forward and right clothoid turn CT2: since the μ-tangency between 703E Cr+(q1) and 702E Cl+(qf) implies that the orientation of 716E q2 is
thus the CT2 has a deflection δ2=θ1−θ+α−μ−π;
3. A backward and left clothoid turn CT3 with a deflection
Existence conditions: the triangle Ω1Ω3Ω2 be valid. That is
2RΩ(1−cos μ)≤L(Ω1Ω2)≤2RΩ(1+cos μ). (17)
Validation conditions are: absolute values of δ1, δ2 and δ3 are less than π+2δc.
Let Ω1 and Ω2 denote centers of CC circles 701F Cl+(q0) and 702F Cr−(qf), respectively; and let θ denote an angle 711F between a line 705F Ω1Ω2 and the positive x-axis. The intermediate configurations q1, q2 and q3 along the path are depicted in
L(Ω1Ω3)=L(Ω2Ω4)=2RΩ cos μ
L(Ω3Ω4)=2RΩ. (18)
To facilitate determination of the path and improve optimality of the resultant path, a geometric constraint is imposed as that a line 707F Ω3Ω4 is parallel to the line 705F, making Ω1Ω3Ω4Ω2 an isosceles trapezoid. Let a denote an angle 712F between the line 705F Ω1Ω2 and a line 706F Ω1Ω3, geometric analysis implies that
Coordinates of Ω3 and Ω4 are respectively given by
Let θ1 represent an angle 713F between the line 708F Ω2Ω4 and the positive x-axis, the canonical L+Ru+Lu−R− path can then be formed by sequentially combining
1. A forward and left clothoid turn CT1: the μ-tangency condition between 701F Cl+(q0) and 703F Cr+(q1) implies that the orientation of 715F q1 is
thus the CT1 has a deflection
2. A forward and right clothoid turn CT2: the μ-tangency condition between 703F Cr+(q1) and 704F Cl−(q2) implies that the orientation of 716F q2 is
thus a deflection of CT2 is u=α+μ−π;
3. A backward and left clothoid turn CT3 with a deflection u=α+μ−π;
4. A backward and right clothoid turn CT4: the μ-tangency condition between 704F Cl−(q2) and 702F Cr+(qf) implies that the orientation of 717F q3 is
thus a deflection of CT4 is
Existence conditions: the angle 712F α is well defined, thus
2RΩ≤L(Ω1Ω2)≤2RΩ cos μ+4RΩ.
Validation conditions are: absolute values of δ1, δ2, δ3 and δ4 are less than π+2δc.
Let Ω1 and Ω2 denote centers of CC circles 701G Cl+(q0) and 702G Cr−(qf) associated with 700G q0 and 799G qf, respectively. Let θ denote an angle 711G between a line 705G Ω1Ω2 and the positive x-axis. As shown in
L(Ω1Ω3)=L(Ω2Ω4)=2RΩ cos μ. (21)
Furthermore, the μ-tangency between 703G Cr−(q1) and 704G Cl−(q2) implies that
L(Ω3Ω4)=2RΩ. (22)
Some embodiments prove a geometric constraint, which is always valid: a line 706G Ω1Ω3 is parallel to a line 708G Ω2Ω4. This realization facilitates efficient computation of coordinates of Ω3 and Ω4, making Ω1Ω4Ω2Ω3 a parallelogram. Let α denote an angle 712G between a line 705G Ω1Ω2 and the line 706G Ω1Ω3. Applying law of cosines in a triangle Ω1q2Ω3 yields
Coordinates of Ω3 and Ω4 are obtained as:
As illustrated in
1. A left clothoid turn CT1 with a deflection
2. A backward and right clothoid turn CT2 with a deflection u=θ1−θ−α−μ;
3. A backward and left clothoid turn CT3 with a deflection −u.
4. A forward and right clothoid turn CT4 with a deflection
Existence conditions: the triangle Ω1q2Ω3 be valid, i.e.,
|2RΩ−4RΩ cos μ|≤(Ω1Ω2)≤2RΩ+4RΩ cos μ.
Validation conditions are: absolute values of δ1, δ2, δ3 and δ4 are less than π+2δc.
according to some embodiments. For example,
Let θ denote an angle 711H between a line 705H Ω1Ω2 and the positive x-axis. Denote a center of a CC circle 703H Cr−(q1) as Ω3. This invention realizes that the following fact holds: Ω1, Ω2 and Ω3 stay in the same straight line 705H. This fact allows derivation of analytical formula to compute a L+R−S−R− path that connects 700H q0 and 799H qf.
The L+R−S−R− path can thus be computed immediately
1. A forward and left clothoid turn CT1 with a deflection
2. A backward and right clothoid turn CT2 with a deflection
3. A straight line segment 704H q2q3 with a length given by
4. A second backward and right clothoid turn CT3 with a deflection δ3=θf−θ−π.
Existence conditions: L(q2q3)≥0, that is
L(Ω1Ω2)≥2RΩ (cos μ+sin μ).
Validation conditions are: absolute values of δ1 and δ2 are less than π+2δc.
according to some embodiments. For example,
Let θ denote an angle between a line Ω1Ω2 and the positive x-axis. Let 715I q1, 716I q2 and 717I q3 represent beginning configurations of a first backward right clothoid turn, a line segment 704I and a second backward right clothoid turn, respectively. Let Ω3 be a center of a CC circle 703I Cr−(q1), and α denote an angle 712I between a line 706I Ω1Ω3 and a line 705I Ω1Ω2. We extend the line 706I Ω1Ω3 to Ω0 such that a line 707I Ω2Ω0 is perpendicular to Ω1Ω0. Let a line Ω3A be perpendicular to the line 704I q2q3, and let B be an intersecting point of the line 707I Ω2Ω0 and the line 704I q2q3. Based on the μ-tangency between 701I Cl+(q0) and 703I Cr−(q1), orientations of both 716I q2 and 717I q3 are equal to θ+α+π, which implies that the line 704I q2q3 is parallel to the line 706I Ω1Ω0 and is perpendicular to the line 707I Ω0Ω2. This indicates that Ω3ABΩ0 is a rectangle. The μ-tangency between the line 704I q2q3 and 703I Cr−(q1), and between the line 704I q2q3 and 702I Cl+(qf) suggests
L(Ω3A)=L(Ω2B)=L(Ω0B)=RΩ cos μ,
which implies that, in a right triangle Ω0Ω1Ω2,
It follows from (24) that the L+R−S−L− canonical CC path includes
1. A forward and left clothoid turn CT1 with a deflection
2. A backward and right clothoid turn CT2 with a deflection
3. A straight line segment 704I q2q3 of a length
4. A second backward and right clothoid turn CT3 with a deflection δ3=θf−θ−π.
Existence conditions: L(q2q3)≥0, that is
Validation conditions are: absolute values of δ1, δ2 and δ3 are less than π+2δc.
according to some embodiments. For example,
Let 701J Cl+(q0) and 702J Cr−(qf) be CC circles associated with 700J q0 and 799J qf, whose centers are Ω1 and Ω2, respectively. Let θ denote an angle 711J between a line 705J Ω1Ω2 and the positive x-axis. As shown in
Let a line Ω4Ω0 be perpendicular to the line 706J Ω1Ω3. Since q0q1q2q3q4 forms a canonical L+R−S−L− path from 700J q0 to 718J q4, it follows from the previous reasoning that L(Ω4Ω0)=2RΩ cos μ. Moreover, let Ω5 be a point on the straight line 706J Ω1Ω3 such that a line 708J Ω2Ω5 is perpendicular to Ω1Ω5. The line 707J Ω4Ω2=2RΩ cos μ and is parallel to Ω1Ω5, making Ω0Ω4Ω2Ω5 a square. Therefore, an auxiliary angle α 712J can be determined in a right triangle Ω1δ2Ω5:
It follows immediately from (25) that the L+R−S−L−R+ CC path is formed by
1. A forward and left clothoid turn CT1 with a deflection
2. A backward and right clothoid turn CT2 with a deflection
3. A backward line segment 755J q2q3 of length
4. A backward and left clothoid turn CT3 with a deflection
5. A forward and right clothoid turn CT4 with a deflection
Existence conditions: L(q2q3)≥0, that is
Validation conditions are: absolute values of δ1, δ2, δ3 and δ4 are less than π+2δc.
Similar to
according to some embodiments. For example,
according to some embodiments. For example,
according to some embodiments. For example,
The system 1000 can include one or combination of a camera 1010, an inertial measurement unit (IMU) 1030, a processor 1050, a memory 1060, a transceiver 1070, and a display/screen 1080, which can be operatively coupled to other components through connections 1020. The connections 1020 can comprise buses, lines, fibers, links or combination thereof.
The transceiver 1070 can, for example, include a transmitter enabled to transmit one or more signals over one or more types of wireless communication networks and a receiver to receive one or more signals transmitted over the one or more types of wireless communication networks. The transceiver 1070 can permit communication with wireless networks based on a variety of technologies such as, but not limited to, femtocells, Wi-Fi networks or Wireless Local Area Networks (WLANs), which may be based on the IEEE 802.11 family of standards, Wireless Personal Area Networks (WPANS) such Bluetooth, Near Field Communication (NFC), networks based on the IEEE 802.15x family of standards, and/or Wireless Wide Area Networks (WWANs) such as LTE, WiMAX, etc. The system 400 can also include one or more ports for communicating over wired networks.
In some embodiments, the system 1000 can comprise image sensors such as CCD or CMOS sensors, lasers and/or camera 1010, which are hereinafter referred to as “sensor 1010”. For example, the sensor 1010 can convert an optical image into an electronic or digital image and can send acquired images to processor 1050. Additionally, or alternatively, the sensor 1010 can sense the light reflected from a target object in a scene and submit the intensities of the captured light to the processor 1050.
For example, the sensor 1010 can include color or grayscale cameras, which provide “color information.” The term “color information” as used herein refers to color and/or grayscale information. In general, as used herein, a color image or color information can be viewed as comprising 1 to N channels, where N is some integer dependent on the color space being used to store the image. For example, an RGB image comprises three channels, with one channel each for Red, Blue and Green information.
For example, the sensor 1010 can include a depth sensor for providing “depth information.” The depth information can be acquired in a variety of ways using depth sensors. The term “depth sensor” is used to refer to functional units that may be used to obtain depth information independently and/or in conjunction with some other cameras. For example, in some embodiments, the depth sensor and the optical camera can be part of the sensor 1010. For example, in some embodiments, the sensor 1010 includes RGBD cameras, which may capture per-pixel depth (D) information when the depth sensor is enabled, in addition to color (RGB) images.
As another example, in some embodiments, the sensor 1010 can include a 3D Time Of Flight (3DTOF) camera. In embodiments with 3DTOF camera, the depth sensor can take the form of a strobe light coupled to the 3DTOF camera, which can illuminate objects in a scene and reflected light can be captured by a CCD/CMOS sensor in the sensor 410. Depth information can be obtained by measuring the time that the light pulses take to travel to the objects and back to the sensor.
As a further example, the depth sensor can take the form of a light source coupled to the sensor 1010. In one embodiment, the light source projects a structured or textured light pattern, which can include one or more narrow bands of light, onto objects in a scene. Depth information is obtained by exploiting geometrical distortions of the projected pattern caused by the surface shape of the object. One embodiment determines depth information from stereo sensors such as a combination of an infra-red structured light projector and an infra-red camera registered to a RGB camera.
In some embodiments, the sensor 1010 includes stereoscopic cameras. For example, a depth sensor may form part of a passive stereo vision sensor, which may use two or more cameras to obtain depth information for a scene. The pixel coordinates of points common to both cameras in a captured scene may be used along with camera pose information and/or triangulation techniques to obtain per-pixel depth information.
In some embodiments, the system 1000 can be operatively connected to multiple sensors 1010, such as dual front cameras and/or a front and rear-facing cameras, which may also incorporate various sensors. In some embodiments, the sensors 1010 can capture both still and video images. In some embodiments, the sensor 1010 can include RGBD or stereoscopic video cameras capable of capturing images at, e.g., 30 frames per second (fps). In one embodiment, images captured by the sensor 1010 can be in a raw uncompressed format and can be compressed prior to being processed and/or stored in memory 1060. In some embodiments, image compression can be performed by the processor 1050 using lossless or lossy compression techniques.
In some embodiments, the processor 1050 can also receive input from IMU 1030. In other embodiments, the IMU 1030 can comprise 3-axis accelerometer(s), 3-axis gyroscope(s), and/or magnetometer(s). The IMU 1030 can provide velocity, orientation, and/or other position related information to the processor 1050. In some embodiments, the IMU 1030 can output measured information in synchronization with the capture of each image frame by the sensor 1010. In some embodiments, the output of the IMU 1030 is used in part by the processor 1050 to fuse the sensor measurements and/or to further process the fused measurements.
The system 1000 can also include a screen or display 1080 rendering images, such as color and/or depth images. In some embodiments, the display 1080 can be used to display live images captured by the sensor 1010, fused images, augmented reality (AR) images, graphical user interfaces (GUIs), and other program outputs. In some embodiments, the display 1080 can include and/or be housed with a touchscreen to permit users to input data via some combination of virtual keyboards, icons, menus, or other GUIs, user gestures and/or input devices such as styli and other writing implements. In some embodiments, the display 1080 can be implemented using a liquid crystal display (LCD) display or a light emitting diode (LED) display, such as an organic LED (OLED) display. In other embodiments, the display 480 can be a wearable display. In some embodiments, the result of the fusion can be rendered on the display 1080 or submitted to different applications that can be internal or external to the system 1000.
Exemplary system 1000 can also be modified in various ways in a manner consistent with the disclosure, such as, by adding, combining, or omitting one or more of the functional blocks shown. For example, in some configurations, the system 1000 does not include the IMU 1030 or the transceiver 1070. Further, in certain example implementations, the system 1000 include a variety of other sensors (not shown) such as an ambient light sensor, microphones, acoustic sensors, ultrasonic sensors, laser range finders, etc. In some embodiments, portions of the system 400 take the form of one or more chipsets, and/or the like.
The processor 1050 can be implemented using a combination of hardware, firmware, and software. The processor 1050 can represent one or more circuits configurable to perform at least a portion of a computing procedure or process related to sensor fusion and/or methods for further processing the fused measurements. The processor 1050 retrieves instructions and/or data from memory 1060. The processor 1050 can be implemented using one or more application specific integrated circuits (ASICs), central and/or graphical processing units (CPUs and/or GPUs), digital signal processors (DSPs), digital signal processing devices (DSPDs), programmable logic devices (PLDs), field programmable gate arrays (FPGAs), controllers, micro-controllers, microprocessors, embedded processor cores, electronic devices, other electronic units designed to perform the functions described herein, or a combination thereof.
The memory 1060 can be implemented within the processor 1050 and/or external to the processor 1050. As used herein the term “memory” refers to any type of long term, short term, volatile, nonvolatile, or other memory and is not to be limited to any particular type of memory or number of memories, or type of physical media upon which memory is stored. In some embodiments, the memory 1060 holds program codes that facilitate the automated parking.
For example, the memory 1060 can store the measurements of the sensors, such as still images, depth information, video frames, program results, as well as data provided by the IMU 1030 and other sensors. The memory 1060 can store a memory storing a geometry of the vehicle, a map of the parking space, a kinematic model of the vehicle, and a dynamic model of the vehicle. In general, the memory 1060 can represent any data storage mechanism. The memory 1060 can include, for example, a primary memory and/or a secondary memory. The primary memory can include, for example, a random access memory, read only memory, etc. While illustrated in
Secondary memory can include, for example, the same or similar type of memory as primary memory and/or one or more data storage devices or systems, such as, for example, flash/USB memory drives, memory card drives, disk drives, optical disc drives, tape drives, solid state drives, hybrid drives etc. In certain implementations, secondary memory can be operatively receptive of, or otherwise configurable to a non-transitory computer-readable medium in a removable media drive (not shown). In some embodiments, the non-transitory computer readable medium forms part of the memory 1060 and/or the processor 1050.
The above-described embodiments of the present invention can be implemented in any of numerous ways. For example, the embodiments may be implemented using hardware, software or a combination thereof. When implemented in software, the software code can be executed on any suitable processor or collection of processors, whether provided in a single computer or distributed among multiple computers. Such processors may be implemented as integrated circuits, with one or more processors in an integrated circuit component. Though, a processor may be implemented using circuitry in any suitable format.
Also, the embodiments of the invention may be embodied as a method, of which an example has been provided. The acts performed as part of the method may be ordered in any suitable way. Accordingly, embodiments may be constructed in which acts are performed in an order different than illustrated, which may include performing some acts simultaneously, even though shown as sequential acts in illustrative embodiments.
Use of ordinal terms such as “first,” “second,” in the claims to modify a claim element does not by itself connote any priority, precedence, or order of one claim element over another or the temporal order in which acts of a method are performed, but are used merely as labels to distinguish one claim element having a certain name from another element having a same name (but for use of the ordinal term) to distinguish the claim elements.
Although the invention has been described by way of examples of preferred embodiments, it is to be understood that various other adaptations and modifications can be made within the spirit and scope of the invention.
Therefore, it is the object of the appended claims to cover all such variations and modifications as come within the true spirit and scope of the invention.