A parking motion planning method for four-wheel steering vehicles in narrow spaces

Abstract

The present application relates to a kind of narrow space parking motion planning method for four-wheel steering vehicle, comprising: the four-wheel steering kinematic model of vehicle is constructed;Based on the four-wheel steering kinematic model, the node expansion process of Hybrid A* algorithm is carried out, for searching the coarse trajectory of starting point to endpoint with four-wheel steering characteristics;Parking is constructed as nonlinear optimization problem, uses OBCA algorithm, and carries out numerical solution with coarse trajectory as initial solution, obtains parking motion trajectory;According to parking motion trajectory, corresponding control vehicle completes parking.Compared with prior art, one aspect of the present application improves Hybrid A* algorithm according to four-wheel steering kinematics, and on the other hand, four-wheel steering kinematics is applied to the nonlinear optimization of parking, and precise obstacle avoidance constraint is carried out using OBCA algorithm, can make full use of the flexible maneuvering ability of four-wheel steering vehicle to carry out the parking planning of narrow space, effectively improve parking efficiency, and efficiency increases with the increase of maximum rear wheel rotation angle.

Description

Technical Field

[0001] The present invention relates to the technical field of vehicle motion planning, and in particular to a narrow space parking motion planning method for a four-wheel steering vehicle. Background Art

[0002] Current motion planning techniques are mostly based on front-wheel steering vehicles. However, due to vehicle nonholonomic constraints, front-wheel steering vehicles can only travel along circular arcs, making parking difficult in confined spaces. Planning trajectories that can track and accurately avoid obstacles is essential, often requiring multiple maneuvers.

[0003] Existing parking motion planning research is primarily categorized into optimization and non-optimization methods. Among non-optimization methods, search-based approaches, such as Hybrid A*, are widely used to find the shortest path by calculating costs. However, these methods only consider front-wheel steering kinematics when expanding nodes, making them inapplicable to four-wheel steering vehicles to improve parking efficiency in confined spaces. Optimization methods have become a popular parking planning framework due to their ability to explicitly model vehicle models, obstacle avoidance constraints, and expectations, enabling them to handle a variety of complex scenarios. However, most studies have used only front-wheel steering kinematic models and are unable to leverage the rear-wheel steering capabilities of four-wheel steering vehicles to improve flexibility. Summary of the Invention

[0004] The purpose of the present invention is to overcome the defects of the above-mentioned prior art and provide a narrow space parking motion planning method for four-wheel steering vehicles, which can improve the efficiency of narrow space parking and achieve flexible and accurate obstacle avoidance.

[0005] The object of the present invention can be achieved by the following technical solution: A method for planning parking motion in a narrow space for a four-wheel steering vehicle, comprising the following steps:

[0006] S1. Construct a four-wheel steering kinematic model of the vehicle;

[0007] S2. Based on the four-wheel steering kinematic model, the node expansion process of the Hybrid A* algorithm is performed to search for a rough trajectory with four-wheel steering characteristics from the starting point to the end point;

[0008] S3. Frame parking as a nonlinear optimization problem and use the OBCA (Optimization-based Collision Avoidance) algorithm to numerically solve the rough trajectory as the initial solution to obtain the parking trajectory.

[0009] S4. Control the vehicle accordingly to complete parking according to the parking motion trajectory.

[0010] Furthermore, the step S1 specifically establishes a four-wheel steering kinematic model of the vehicle with the rear axle center as a reference point:

[0011]

[0012] in, is the vehicle state, [x,y] T is the coordinate of the rear axle center, θ is the heading angle, v is the longitudinal velocity at the rear axle center, are the front and rear wheel steering angles, U=[a,ω f ,ω r ] T is the vehicle control input, a is the rear axle center acceleration, ω f ,ω r are the steering angular velocities of the front and rear axles respectively, and L is the wheelbase.

[0013] Furthermore, the specific process of step S2 includes:

[0014] Fixed sampling values ​​are set for the front and rear wheels within their maximum turning angle ranges. When the front and rear wheel turning angles are equal and in the same direction, the vehicle performs a crab maneuver. Each node expansion process expands N nodes in each direction of travel (forward or backward).

[0015] The forward Euler method is used to discretize the four-wheel steering kinematic model. Assuming that the expansion distance is short enough, the node state transfer equation can be obtained to determine the state of each new node.

[0016] Furthermore, the node state transfer equation is as follows:

[0017]

[0018] Among them, s is the distance of each node expansion. By applying the node state transfer equation during the node expansion process, the state of each new node can be obtained based on the parent node.

[0019] Furthermore, the sampling value of the front wheel angle is set to The sampling value of the rear wheel angle is set to

[0020] Furthermore, the process of constructing parking as a nonlinear optimization problem in step S3 includes:

[0021] S31. Use the OBCA algorithm to mathematically model the obstacle and obstacle avoidance relationship to obtain an obstacle avoidance model;

[0022] S32, supplement constraints and objective functions to construct a complete parking nonlinear optimization problem.

[0023] Further, the obstacle avoidance model in the step S31 is:

[0024]

[0025]

[0026]

[0027]

[0028] for i = 0, …, N, m = 1, …, M

[0029] where λ, μ are dual variables, A (m) , b (m) is a matrix describing the boundary of the m-th obstacle, g = [d, d, f l , rh] T define the initial rectangular boundary of ego vehicle, d represents half of the vehicle width, fl, rh are the distances from the rear axle to the front and rear boundaries of the vehicle respectively, is a rotation matrix, t(ζ) = [x, y] T is a translation vector, N is the total number of discrete steps, M is the number of obstacles, d min is the minimum allowed distance between ego vehicle and obstacles.

[0030] Further, the four-wheel steering kinematics is used as the vehicle nonholonomic constraint in the step S32.

[0031] Further, the objective function in the step S32 is to minimize the parking termination time, while setting penalty terms for the front wheel steering angular velocity, the rear wheel steering angular velocity, and the vehicle acceleration.

[0032] Further, the complete parking nonlinear optimization problem in the step S32 is specifically:

[0033]

[0034] s.t. F model (ξ k , u k ) = 0

[0035] ξ0= ξ start , ξ N = ξ goal

[0036] ξ min ≤ ξ k ≤ ξ max , u min ≤ u k ≤ u max

[0037]

[0038]

[0039]

[0040] forgood=0,…,N,m=1,…,M

[0041] Among them, the variable t f is the parking end time, coe f ,coe r ,coe a are the coefficients for penalizing the front wheel steering angular velocity, rear wheel steering angular velocity, and vehicle acceleration terms respectively.

[0042] Compared with the prior art, the present invention has the following advantages:

[0043] The present invention improves the Hybrid A* algorithm based on four-wheel steering kinematics to obtain a new Hybrid A*-4WS (Four-Wheel-Steer) algorithm, which can search for a rough trajectory from the starting point to the parking space and has four-wheel steering characteristics such as "crab walking". The four-wheel steering kinematics are applied to the nonlinear optimization of parking, which can fully utilize the maneuverability of the 4WS vehicle and use the OBCA algorithm to accurately avoid obstacles. In this way, the four-wheel steering kinematic model is integrated with the motion planning algorithm, which fully utilizes the flexible maneuverability of the four-wheel steering vehicle, can effectively improve the efficiency of parking in narrow spaces, and at the same time ensure accurate obstacle avoidance.

[0044] The present invention improves the node expansion mechanism of the Hybrid A* algorithm based on the kinematics of four-wheel steering and the corresponding vehicle structural parameters. Fixed sampling values ​​are set for the front and rear wheels within their maximum turning angle ranges, and a crab-like maneuver is performed when the front and rear wheel angles are equal and in the same direction. Each node expansion process expands N nodes at a time in each direction of travel (forward or backward). Assuming the expansion distance is short enough, the state of each new node can be obtained by integrating the 4WS kinematic model. The improved Hybrid A*-4WS algorithm can search for a coarse trajectory from the starting point to the berth (i.e., the end point), greatly improving the search capability.

[0045] The present invention constructs parking as a nonlinear optimization problem, in which the vehicle nonholonomic constraints are combined with four-wheel steering kinematics to fully utilize the maneuverability of the 4WS vehicle. In order to achieve the shortest possible parking time, the variable t is introduced when constructing the minimization objective function. fThe parking end time is represented by t end, a penalty on the size of the control variables (front wheel steering angular velocity, rear wheel steering angular velocity, vehicle acceleration) is also considered, and the OBCA obstacle avoidance algorithm is used to mathematically model the obstacles and the obstacle avoidance relationship, based on the coarse trajectory as an initial solution, the final feasible, smooth, collision-free and shortest time parking motion trajectory can be obtained, and the parking efficiency is effectively improved, and the efficiency is improved with the increase of the maximum rear wheel steering angle. BRIEF DESCRIPTION OF DRAWINGS

[0046] Figure 1 A flowchart of the method of the present application is shown in Figure 1.

[0047] Figure 2 A schematic diagram of the kinematic model of a four-wheel steering vehicle is shown in Figure 2.

[0048] Figure 3 A pseudo-code diagram of the improved Hybrid A*-4WS algorithm of the present application is shown in Figure 3.

[0049] Figure 4 A schematic diagram of the solution process of the parking motion trajectory is shown in Figure 4. DETAILED DESCRIPTION

[0050] The present application will be described in detail below with reference to the accompanying drawings and specific embodiments.

[0051] EMBODIMENT

[0052] As shown in Figure 1, a narrow space parking motion planning method for a four-wheel steering vehicle includes the following steps: Figure 1

[0053] S1, a four-wheel steering kinematic model of the vehicle is constructed;

[0054] S2, based on the four-wheel steering kinematic model, the node expansion process of the Hybrid A* algorithm is performed to search for a coarse trajectory with four-wheel steering characteristics from the starting point to the end point;

[0055] S3, the parking is constructed as a nonlinear optimization problem, the OBCA algorithm is used, and the coarse trajectory is used as an initial solution for numerical solution to obtain the parking motion trajectory;

[0056] S4, according to the parking motion trajectory, the vehicle completes parking accordingly.

[0057] The present embodiment applies the above-mentioned scheme, mainly including:

[0058] I. The Hybrid A* algorithm is improved according to the kinematics of the four-wheel steering vehicle to obtain the Hybrid A*-4WS algorithm.

[0059] Specifically, it includes:

[0060] ​1.1. Establish a simple kinematic model of a four-wheel steering vehicle with the rear axle center as the reference point;

[0061] 1.2. Improve the node expansion mechanism of the search algorithm;

[0062] 1.3. The improved node expansion function is integrated into the original algorithm to obtain Hybrid A*-4WS.

[0063] like Figure 2 As shown in Figure 2, the kinematic model of the four-wheel steering vehicle established in step 1.1 is:

[0064]

[0065] in, is the vehicle state, [x,y] T is the coordinate of the rear axle center, θ is the heading angle, v is the longitudinal velocity at the rear axle center, are the front and rear wheel steering angles respectively. f ,ω r ] T is the vehicle control input, a is the rear axle center acceleration, ω f ,ω r are the steering angular velocities of the front and rear axles respectively, and L is the wheelbase.

[0066] In step 1.2, fixed sampling values ​​are set for the front and rear wheels within their maximum turning angle ranges, where the sampling value of the front wheel turning angle is set to The sampling value of the rear wheel angle is set to When the front and rear wheel turning angles are equal and in the same direction, the vehicle performs crab maneuvering, and each node expansion process expands 15 nodes in each direction of travel (forward or backward).

[0067] The forward Euler method is used to discretize the kinematic model of the four-wheel steering vehicle. Assuming that the extension distance is short enough, the node state transfer equation can be obtained as follows:

[0068]

[0069] Among them, s is the distance of each node expansion. By applying this equation during the node expansion process, the state of each new node can be obtained based on the parent node.

[0070] In step 1.3, if Figure 3 As shown in the figure, the steps of the Hybrid A*-4WS algorithm are as follows:

[0071] Step 1, initialize openlist to the starting node and closedlist to the empty set.

[0072] Step 2: Determine whether openlist is an empty set. If it is, continue to loop and execute subsequent steps. Otherwise, end the algorithm.

[0073] Step 3: Select the node with the lowest fcost in the openlist as the current node. If the current node can reach the end point using the RS curve after passing the AnalyticExpansion, connect the two nodes and make the parent node of the end point point to the current node. Otherwise, remove the current node from the openlist and add it to the closedlist.

[0074] Step 4: Expand the current node with the parent node as the parent node. For each child node, if it is not in the closed list, check the trajectory for collisions and calculate the fcost. If the child node is not in the open list, add it to the set. Otherwise, if it is in the open list and the new fcost is smaller, update the fcost and the corresponding parent node. If it is already in the closed list or there is a collision, discard the child node. Loop back to Step 2.

[0075] Second, the four-wheel steering vehicle kinematics is applied to the nonlinear optimization of parking (i.e., the four-wheel steering kinematics is combined as the vehicle non-holonomic constraint) to fully utilize the maneuverability of the 4WS vehicle and use the OBCA algorithm for accurate obstacle avoidance constraints.

[0076] The specific steps include:

[0077] 2.1. Constructing the kinematic model formula of the four-wheel steering vehicle is as described above.

[0078] 2.2. Use the OBCA obstacle avoidance algorithm to mathematically model obstacles and obstacle avoidance relationships.

[0079] 2.3. Supplement constraints and objective functions to construct a complete parking nonlinear optimization problem.

[0080] In step 2.2, the OBCA obstacle avoidance algorithm accurately handles the interference relationship between objects. The formula is as follows:

[0081]

[0082]

[0083]

[0084]

[0085] for k=0,...,N,m=1,…,M

[0086] Among them, λ, μ are dual variables, A (m),b (m) is the matrix describing the boundary of the mth obstacle, g=[d,d,fl,rh] T Define the initial rectangular boundary of the vehicle, d represents half of the vehicle width, fl and rh are the distances from the rear axle to the front and rear boundaries of the vehicle, respectively. is the rotation matrix, t(ζ) = [x, y] T is the translation vector, N is the total number of discrete steps, M is the number of obstacles, d min is the minimum allowable distance between the vehicle and the obstacle.

[0087] In step 2.3, in order to achieve the shortest possible parking time, the variable t is introduced when constructing the minimization objective function. f To represent the parking termination time. In addition, the penalty for the size of the control variable is also considered. Therefore, the complete optimization problem is as follows:

[0088]

[0089] F model (ξ k ,u k )=0

[0090] ξ0=ξ start ,ξ N =ξ goal

[0091] ξ min ≤ξ k ≤ξ max ,u min ≤u k ≤u max

[0092]

[0093]

[0094]

[0095] forgood=0,…,N,m=1,…,M

[0096] Among them, coe f ,coe r ,coe a are the coefficients for penalizing the front wheel steering angular velocity, rear wheel steering angular velocity, and vehicle acceleration terms respectively.

[0097] 3. Integrate the two algorithms, such as Figure 4 As shown, the specific steps include:

[0098] 3.1. Use the Hybrid A*-4WS algorithm to search for coarse trajectories.

[0099] 3.2. Use the constructed optimization algorithm to smooth the rough trajectory and obtain a feasible parking trajectory with the shortest time and accurate obstacle avoidance.

[0100] In practical applications, first, fixed sampling values ​​are set for the front and rear wheel angles according to the maximum front and rear wheel angles of a four-wheel steering vehicle;

[0101] Then, based on the four-wheel steering kinematics and the corresponding vehicle structural parameters, the node expansion mechanism of the Hybrid A* algorithm is improved;

[0102] Then use the improved Hybrid A*-4WS algorithm to calculate the rough trajectory from the starting point to the berth;

[0103] Finally, the rough trajectory is passed to the optimizer as the initial solution, and the final shortest time parking planning trajectory is obtained through calculations with constraints and expectations.

[0104] Compared to existing technologies, this solution leverages the unique advantages of four-wheel steering (FWS) vehicles' maneuverability and flexibility to design a parking motion planning method for narrow spaces with FWS, enhancing parking efficiency. This approach incorporates an improved node expansion Hybrid A*-4WS algorithm and an optimization algorithm utilizing a FWS model and the OBCA obstacle avoidance algorithm. The integration of these two approaches significantly improves parking distance and time.

[0105] This solution has the following advantages:

[0106] 1. Improving the search capability of the search algorithm: The Hybrid A* algorithm was improved based on the kinematics of four-wheel steering, resulting in Hybrid A*-4WS. This algorithm can search for a rough trajectory from the starting point to the berth and incorporates four-wheel steering features such as "crab walking" to improve search capabilities.

[0107] Second, improving parking efficiency: Four-wheel steering kinematics are applied to nonlinear parking optimization to fully utilize the maneuverability of the 4WS vehicle, and the OBCA algorithm is used for precise obstacle avoidance constraints. By integrating the two algorithms, the coarse trajectory obtained by Hybrid A*-4WS is passed as the initial guess to the optimizer, resulting in a final feasible, smooth, and collision-free trajectory. This effectively improves parking efficiency, which increases with increasing maximum rear wheel steering angle.

Claims

1. A parking motion planning method for a four-wheel steering vehicle in a narrow space, characterized in that: The following steps are involved: S1. Construct a four-wheel steering kinematic model of the vehicle; S2. Based on the four-wheel steering kinematic model, the node expansion process of the Hybrid A* algorithm is performed to search for a rough trajectory with four-wheel steering characteristics from the starting point to the end point; S3. Construct parking as a nonlinear optimization problem, use the OBCA algorithm, and numerically solve it with the rough trajectory as the initial solution to obtain the parking trajectory; S4. Control the vehicle accordingly to complete parking according to the parking motion trajectory; The specific process of step S2 includes: Fixed sampling values ​​are set for the front and rear wheels within their maximum turning angle ranges. When the front and rear wheel turning angles are equal and in the same direction, the vehicle performs a crab maneuver. Each node expansion process expands N nodes in each direction of travel, where the direction of travel includes forward or backward. The forward Euler method is used to discretize the four-wheel steering kinematic model. Assuming the expansion distance is short enough, the node state transfer equation can be obtained to determine the state of each new node. The process of constructing parking as a nonlinear optimization problem in step S3 includes: S31. Use the OBCA algorithm to mathematically model the obstacle and obstacle avoidance relationship to obtain an obstacle avoidance model; S32, supplement constraints and objective functions to construct a complete parking nonlinear optimization problem; In step S32, four-wheel steering kinematics is used as a vehicle nonholonomic constraint. The objective function in step S32 is to minimize the parking termination time, and penalty terms are set for the front wheel steering angular velocity, the rear wheel steering angular velocity, and the vehicle acceleration. The complete parking nonlinear optimization problem in step S32 is specifically: ， ， ， ， ， ， ， ， Among them, the variable The parking end time is are the coefficients for penalizing the front wheel steering angular velocity, rear wheel steering angular velocity, and vehicle acceleration terms, is the total number of discrete steps, is the number of obstacles, is the minimum allowable distance between the vehicle and the obstacle, is the dual variable, is the matrix describing the boundary of the mth obstacle, is the vehicle control input, is the rear axle center acceleration, are the front and rear axle steering angular velocities, Define the initial rectangular boundary of the ego vehicle, Represents half the width of the vehicle, are the distances from the rear axle to the front and rear boundaries of the vehicle, is the rotation matrix, is the translation vector, is the vehicle state, is the center coordinate of the rear axle, is the heading angle, is the longitudinal velocity at the center of the rear axle, are the front and rear wheel steering angles respectively.

2. The method for planning parking motion in a narrow space for a four-wheel steering vehicle according to claim 1, characterized in that: The step S1 specifically establishes a four-wheel steering kinematic model of the vehicle with the rear axle center as a reference point: ， in, is the vehicle state, is the center coordinate of the rear axle, is the heading angle, is the longitudinal velocity at the center of the rear axle, are the front and rear wheel steering angles, is the vehicle control input, is the rear axle center acceleration, are the front and rear axle steering angular velocities, is the wheelbase.

3. The method for planning parking motion in a narrow space for a four-wheel steering vehicle according to claim 1, characterized in that: The node state transfer equation is as follows: ， in, is the distance of each node expansion, k is the current node, k +1 is a new node, φ r is the rear wheel turning angle, φ f is the front wheel turning angle. By applying the node state transfer equation during the node expansion process, the state of each new node can be obtained according to the parent node.

4. The method for planning parking motion in a narrow space for a four-wheel steering vehicle according to claim 3, characterized in that: The sampling value of the front wheel angle is set to , the sampling value of the rear wheel angle is set to ,in,- φ f,max is the maximum value of the front wheel turning angle backward, φ f,max is the maximum forward turning angle of the front wheel, — φ r,max is the maximum rear wheel turning angle, φ r,max The maximum forward rotation angle of the rear wheel.

5. The method for planning parking motion in a narrow space for a four-wheel steering vehicle according to claim 1, characterized in that: The obstacle avoidance model in step S31 is: ， ， ， ， ， in, is the dual variable, is the matrix describing the boundary of the mth obstacle, Define the initial rectangular boundary of the ego vehicle, Represents half the width of the vehicle, are the distances from the rear axle to the front and rear boundaries of the vehicle, is the rotation matrix, is the translation vector, is the total number of discrete steps, is the number of obstacles, is the minimum allowable distance between the vehicle and the obstacle.

Images