Rear wheel steering vehicle track planning method and system in road scene based on iterative optimization

Abstract

The invention provides a rear wheel steering vehicle track planning method and system in a road scene based on iterative optimization, and the method comprises the steps: S1, building an optimization problem, and deducing a kinematics formula of a rear wheel steering vehicle, an obstacle avoidance constraint formula, and a calculation formula of the distance and angle error between a rear axle center point and a road center line; s2, on the basis of a road scene structure, constructing a collision-free track which fits a road center line as much as possible to serve as an initial guess in an optimization problem; s3, solving an approximate feasible track, softening the kinematics constraint into a penalty function, constructing a space-time corridor constraint, and constructing and solving an optimization problem to obtain the approximate kinematics feasible track; and S4, solving an optimal track, and gradually increasing sampling points in the optimization problem until the number of the sampling points reaches a threshold value. The method is combined with a vehicle positioning algorithm, track planning of the rear wheel steering vehicle in a road scene can be realized, scene robustness is realized, and the quality of the planned track is high.

Description

Technical Field

[0001] The present invention relates to the field of robotics, and in particular to a method and system for rear-wheel steering vehicle trajectory planning in a road scene based on iterative optimization. Background Art

[0002] With the development, popularization and application of autonomous driving technology, driverless vehicles are gradually coming into our field of vision. The automatic driving of driverless vehicles on the road is a key technology for autonomous driving. However, there are often a large number of static obstacles and small spaces in these scenes. Rear-wheel steering vehicles have a small turning radius and are suitable for driving in narrow scenes, but they need to control the front and rear wheels at the same time, and the control dimension is high, which brings challenges to the path planning and motion control of mobile robots. Therefore, how to plan a smooth trajectory close to the center line of the road in a short time is a key technology in the field of autonomous driving.

[0003] Domestic and foreign researchers have also done a lot of research on the path planning problem of unmanned vehicles, which can be roughly divided into two categories:

[0004] The first is the method based on sampling search. Researchers generally sample in the state space or control space, and then search for paths through search methods such as A* and dynamic programming. For example, Evestedt et al. [1] (Evestedt, Niclas, Oskar Ljungqvist, and Daniel Axehill. "Motion planning for a reversing general 2-trailer configuration using Closed-Loop RRT." 2016 IEEE / RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2016.) used a fast random expansion tree to search for feasible paths and implement vehicle trajectory planning. The Chinese patent with publication number CN117302187A establishes a Frenet coordinate system with the center line of the lane as the axis, sets a fixed step size for road point sampling based on the bicycle model, and outputs road sampling point data information in the vehicle's drivable area. The method based on sampling search has a faster planning speed, but its output trajectory cannot guarantee optimality. At the same time, when the scene is narrow, planning may fail.

[0005] The second is the trajectory planning method based on optimization. This method first models the trajectory planning problem as an optimal control problem, and then solves the optimal control problem to obtain the optimal trajectory. The Chinese invention patent with publication number CN118163817A uses nonlinear model predictive control to optimize the trajectory points to obtain the final trajectory. Li et al. [2] (Li, Bai, et al. "Maneuver planning for automatic parking with safe travel corridors: A numerical optimal control approach." 2020 European Control Conference (ECC). IEEE, 2020.)c constructs an optimization problem based on space-time corridor constraints and solves it to obtain the vehicle trajectory. The optimization-based method has the advantage of being able to obtain the optimal trajectory, but it has the disadvantages of being time-consuming and having poor real-time performance.

[0006] In order to achieve the task of efficiently planning the optimal path for a rear-wheel steering vehicle in a road scenario, based on the previous work, the present invention innovatively proposes a rear-wheel steering vehicle trajectory planning method in a road scenario based on iterative optimization, which can be applied to the path planning task of a rear-wheel steering vehicle in a road scenario. Summary of the invention

[0007] In view of the defects in the prior art, the object of the present invention is to provide a method and system for rear-wheel steering vehicle trajectory planning in road scenarios based on iterative optimization.

[0008] The rear-wheel steering vehicle trajectory planning method in a road scene based on iterative optimization provided by the present invention includes:

[0009] Step S1: Establish a kinematic model of a rear-wheel steering vehicle, establish an analytical expression for the distance and angle error between the midpoint of the vehicle's rear axle and the centerline of the road based on the geometric relationship in the road scene, and formulate the trajectory planning problem as an optimization problem;

[0010] Step S2: Based on the collision risk between the centerline and surrounding obstacles, the centerline of the road is divided into multiple wide and narrow local sections, and at the same time, for each point in the centerline set, the corresponding local obstacle is found; a local coarse path is planned for each local section, and then combined into a complete global coarse path; based on the global coarse trajectory, the coarse trajectory is derived as the initial guess for the subsequent optimization problem;

[0011] Step S3: For the problem that the global rough trajectory is collision-free but kinematically infeasible, the kinematic constraint is softened into the objective function as a penalty function, and a linear obstacle avoidance constraint is used to construct and solve a simplified optimization problem to obtain an approximate kinematically feasible trajectory;

[0012] Step S4: Based on wide and narrow local road sections and local obstacles, obstacle avoidance constraints are constructed between the vehicle and the local obstacles. By gradually increasing the sampling points in the optimization problem, the optimization problem with a smaller number of sampling points is used to warm-start the subsequent optimization problem with a larger number of sampling points, and the optimal trajectory is iteratively solved until the number of sampling points reaches a threshold, thereby finally realizing the trajectory planning of the rear-wheel steering vehicle in the road scene.

[0013] Preferably, the step S1 comprises:

[0014] Step S1.1: kinematic modeling;

[0015] In the fixed planning time domain T, the number of sampling points is N. The first-order Euler discretization method is used to set the rear axle midpoint position (x0, y0), the heading angle θ, and the wheel steering angle of the rear-wheel steering vehicle. Rear wheel steering angle With the rear wheel speed v as the state quantity, set the front wheel steering angle angular velocity ω f 、Rear wheel steering angle angular velocity ω r With the rear wheel acceleration a as the control variable, the vehicle motion formula under discrete time is obtained as follows:

[0016]

[0017] Where Δt is the sampling time Δt = T / N; s(k) = [x(k), y(k), θ(k), φ f (k),φ r (k),v(k)] T is the state quantity at the kth moment, u(k)=[ω f (k),ω r (k),a(k)] T is the control quantity at the kth moment, Δ is the difference of the state / control quantity at adjacent moments; f(s(k),u(k)) is the kinematic equation of the system;

[0018] At the moment k = 0, ..., N, the front and rear wheel steering angles and angular velocities, rear wheel velocities and accelerations of the vehicle are subject to physical constraints, and the maximum / minimum constraints of the design state quantities and control quantities are as follows:

[0019]

[0020] in, v max They are the maximum front wheel turning angle, maximum rear wheel turning angle and maximum rear wheel linear speed of the vehicle, a max They are the maximum front wheel angular velocity, maximum rear wheel angular velocity and maximum rear wheel acceleration of the vehicle respectively;

[0021] Step S1.2: The state of the vehicle at time k=0 is the current state of the vehicle, and the boundary condition constraints are designed as follows:

[0022] s(0)=s start #(3)

[0023] Step S1.3: In the road scene, use convex polygons to model other vehicles and other obstacles as Among them, Obs j is the jth obstacle, is the 1st,…,Nth j Vertices, N j is the number of vertices of the jth obstacle, N obs is the number of obstacles; the shape of the vehicle is approximated by a rectangle, and the vertices of the vehicle at time k are represented as Veh(k): = {A(k), B(k), C(k), D(k)}, where Veh(k) represents the vertex geometry of the vehicle at time k, and A(k), B(k), C(k), and D(k) represent the four vertices of the rectangle covering the vehicle body; at time k = 0, ..., N, the vehicle needs to avoid collision with obstacles, so the obstacle avoidance constraint is expressed as:

[0024] OutPoly(p,Obs j ),p∈Veh(k),

[0025] OutPoly(q,V eh (k)),q∈Obs j ,#(4)

[0026] k=0,…,N,j=1,…,N obs ,

[0027] OutPoly(a,{b1,…,b n}) function represents the point a in the convex polygon {b1,…,b n}, where n is the number of sides of the polygon;

[0028]

[0029] Among them, S (·) represents the area of ​​a triangle or convex polygon, ∈ is the relaxation parameter, where the area of ​​the convex polygon is obtained by offline calculation. The area calculation formula is:

[0030]

[0031] in, Points a and b i ,b i+1Coordinate values ​​in the two-dimensional plane;

[0032] Step S1.4: Considering the road scenario, the vehicle needs to approach the center line of the road while approaching the target point. Therefore, the objective function expression is:

[0033]

[0034] Where s = [s(0) T ,s(1) T ,…,s(N)] T Represents the state quantity at all times, u=[u(0) T ,u(1) T ,…,u(N-1)] T represents the control quantity at all times, ω g ,ω e ,ω c are the weights of approaching the end point, approaching the center line of the road, and reducing control consumption; x g is the state quantity of the end point, ||e(k)|| 2 =e p (k) 2 +ω θ e θ (k) 2 is the penalty for deviation from the center line of the road at time k, where e p (k) and e θ (k) represents the lateral error and heading angle error between the midpoint of the vehicle's rear axle and the centerline of the road, ω θ is the weight of the heading angle error; the road centerline is discretized into a set in is the set of midpoints of the road centerline, P cen [k] is the position of the kth point in the set, (x cen [k],y cen [k]) are the positions of the kth point in the set on the two-dimensional plane, θ cen [k] is the heading angle of the kth point in the set, is the number of points in the center line set;

[0035] To derive e p (k) and e θ The analytical expression of (k) is as follows: Let the initial guess of the vehicle position in the optimization problem be Where (x init (k),y init (k)) is the initial guess of the vehicle position at time k; then, point (x init (k),y init (k)) Projected onto the road centerline, we get a set of reference points in, is (x init (k),y init (k)) The position of the projection point on the road centerline, and its corresponding relationship is: Assume that the center line of the road is composed of straight line segments and circular arc segments, and the projection point of the variable (x(k), y(k)) is ref (k),y ref (k)) are in the same straight line / arc segment. For the straight line segment, calculate e p (k) and e θ The expression of (k) is:

[0036] e p (k) = (y(k) - y re f (k))cosθ ref (k)-(x(k)-x ref (k))sinθ ref (k)

[0037] e θ (k) = θ(k) - θ ref (k)

[0038] For circular arc segments, calculate and The expression is:

[0039]

[0040] e θ (k) = θ(k) - θ ref (k)+θ d (k)

[0041] Among them, (x R (k),y R (k)) is the position of the center of the arc segment, R(k) is the radius of the arc segment corresponding to time k, θ d (k) is the position of the vehicle at time k (x(k), y(k)) and the position of the reference point (x ref (k),y ref (k)) corresponds to the arc center (x R (k),y R (k)) angle difference.

[0042] Preferably, the step S2 comprises:

[0043] Step S2.1: Segment the centerline and identify local obstacles;

[0044] Step S2.1.1: Classification of points on the centerline and identification of local obstacles;

[0045] For each point on the centerline, it is divided into two categories: wide and narrow according to its collision risk with surrounding obstacles along the centerline; among them, for the kth point P on the centerline cen [k]=(x cen [k],y cen [k],θ cen [k]), if any vehicle position in a certain neighborhood With any obstacles Obs j ,j=1,...,N obs If no collision occurs, it is considered that there is no collision risk; the calculation standard is:

[0046]

[0047] Among them, Obs is the obstacle set {Obs j ,j=1,...,N obs}, CheckCollisionRisk(P cen ,Obs) is the judgment point P cen Function of the collision risk with obstacle Obs; For point P cen The surrounding area is expressed as: where p thre and θ thre are the thresholds for position and angle respectively;

[0048] If there is no collision risk, then point P is considered cen [k]=(x cen [k],y cen [k],θ cen [k]) is classified as wide, otherwise it is classified as narrow; at the same time, according to the distance between the obstacle and point P cen [k], and identify P cen [k] nearby local obstacles, specifically: if the obstacle Obs j A vertex of cen If the distance between [k] is within the threshold Δs, then Obs j Point P cen [k] is in the surrounding obstacle set LocObs[k];

[0049] Step S2.1.2: centerline segmentation;

[0050] Judgment point P cenAfter classification of [k], if it belongs to wide, it is classified into the wide point set, otherwise it is classified into the narrow point set; when the point changes from wide to narrow, the current wide point set is classified into the wide road section set, and the current wide point set is set to the empty set; conversely, when it changes from narrow to wide, the current narrow point set is classified into the narrow road section set, and the current narrow point set is set to the empty set; repeat this operation, traverse the points on the center line, and obtain multiple wide / narrow road sections; among which the i-th road section is represented as Among them, (x[k p ],y[k p ],θ[k p ]) is the kth p The indexed point, p is the index of the point in the set; represents the number of points in the i-th road section;

[0051] Step S2.2: planning a rough path;

[0052] A local coarse path is planned for each road section. For the i-th road section, if it is a wide road section, the road section itself is used as the coarse path. If it is a narrow road section, the coarse path is planned using the A* algorithm. Then, the road sections are connected and combined in sequence to obtain a global coarse path. Then, the global coarse path is spatially resampled to obtain a coarse path. in is the coarse path, N curr is the number of sampling points, x init [k],y init [k] is the position of the kth path point in the plane coordinate system;

[0053] Step S2.3: Calculate the rough trajectory;

[0054] Since the rough trajectory only contains position information, the rough trajectory is calculated based on the obtained rough trajectory What is missing from The expression is as follows:

[0055] θ init (k+1)=tan -1 (Δy init (k),Δx init (k)),k=0,…,N curr -1

[0056] v d (k) = Δx init (k)cosθ init (k)+Δy init (k)sinθ init (k)

[0057] v init (k) = sgn(v d (k))·||(Δx init (k),Δy init (k))|| / Δt c

[0058]

[0059] v init (N curr )=v init (N curr -1)

[0060]

[0061] Among them, x init (k),y i n it (k),θ init (k), v init (k) are respectively x(k), y(k), θ(k), φ at time k f (k),φ r (k), the initial guess of v(k), v d (k) is the speed direction of the vehicle at time k, sgn is the sign function, Δt c =T / N curr The number of sampling points is N curr The sampling time below.

[0062] Preferably, step S3 comprises:

[0063] Step S3.1: design a penalty function to soften the kinematic constraints;

[0064] For the system kinematic constraint Δs(k)=f(s(k),u(k)), the penalty function is designed as: Add the penalty function to the objective function to obtain a new objective function;

[0065] Step S3.2: Design space-time corridor obstacle avoidance constraints;

[0066] For the kth step in the prediction time domain, the predicted robot position is: (x init (k),y init (k));

[0067] For the predicted robot position, initialize the linear obstacle avoidance constraints:

[0068] x min (k) = x init(k) = x max (k)

[0069] y min (k) = y init (k) = y max (k)

[0070] After that, we can loop to get x as follows: min (k),x max (k),y min (k),y max (k):

[0071] x min (k) = x min (k)-γ

[0072] if C(x min (k)-γ:x max (k),y min (k):y max (k)) = 0

[0073] x max (k) = x max (k)+γ

[0074] if C(x min (k):x max (k)+γ,y min (k):y max [k]) = 0

[0075] y min (k) = y min (k)-γ

[0076] if C(x min (k):x max (k),y min (k)-γ:y max (k)) = 0

[0077] x max (k) = x max (k)+γ

[0078] if C(x min (k):x max (k),y min (k):y max (k)+γ)=0

[0079] Among them, γ is the single step length, C(x min :x max ,y min :y max ) is to convert xmin :x max ,y min :y max Function that projects space to a two-dimensional grid map. Its return value is 0 for x min :x max ,y min :y max There are no obstacles in the space, otherwise there are obstacles. The final space-time corridor constraints are as follows:

[0080] x min (k)≤x(k)≤x max (k),

[0081] y min (k)≤y(k)≤y max (k),#(6)

[0082] k=0,…,N

[0083] curr

[0084] Indicates that the mobile robot's position is within the local rectangular frame;

[0085] Step S3.3: solving the approximate feasible trajectory;

[0086] By transforming the kinematic constraints into penalty terms in the objective function and constructing the space-time corridor constraints, the optimal control problem is constructed as follows:

[0087]

[0088] st kinematic constraint formula (2),

[0089] Boundary condition constraint formula (3),

[0090] The space-time corridor constraint formula (6) is:

[0091] Among them, s and u are the variables to be solved. is the initial guess, and a nonlinear solver is used to solve the approximate feasible trajectory.

[0092] Preferably, the step S4 comprises:

[0093] Step S4.1: construct and solve the optimal trajectory under the current sampling point;

[0094] Based on the center line of the road, the obstacle avoidance function is reconstructed, and the initial guess Calculate its projection on the center line For the initial guess point P at time k init (k), if it is classified as wide and is at the projection point Pref (k) Surrounding, i.e. Then the obstacle avoidance constraint is simplified as:

[0095]

[0096] Where P(k) = (x(k), y(k), θ(k)) is the position of the vehicle at time k; otherwise, only the position with respect to the projection point P is considered. ref The obstacle avoidance constraint of the local obstacle Locobs(k) corresponding to (k) is:

[0097]

[0098] Where Δs is the radius of the local area;

[0099] At this point, the optimization problem is constructed as:

[0100] The objective function (5) is:

[0101] st kinematic constraints (1) (2),

[0102] Boundary condition constraint formula (3),

[0103] Simplify the obstacle avoidance constraint formula (7) and formula (8),

[0104] Take the trajectory obtained in step S3.3 is the initial guess, and a nonlinear solver is used to solve the optimal trajectory at the current sampling point.

[0105] Step S4.2: iteratively increase sampling points;

[0106] The optimal trajectory of more sampling points is gradually solved by iteratively increasing the sampling points. In each iteration, the current sampling points are increased to α times the original number, and the optimal trajectory under the original number of sampling points is resampled to the trajectory of the current number of sampling points according to time. This is used as the initial solution. According to the method in step S4.1, the optimal trajectory under the current sampling point is calculated, and this cycle is repeated until the current number of sampling points N curr Reaching the threshold sampling point N thre .

[0107] The rear-wheel steering vehicle trajectory planning system in a road scene based on iterative optimization provided by the present invention includes:

[0108] Module M1: Establish a kinematic model for a rear-wheel steering vehicle. Based on the geometric relationship in the road scene, establish an analytical expression for the distance and angle error between the midpoint of the vehicle's rear axle and the centerline of the road, and formulate the trajectory planning problem as an optimization problem.

[0109] Module M2: Based on the collision risk between the centerline and surrounding obstacles, the road centerline is divided into multiple wide and narrow local sections, and the corresponding local obstacles are found for each point in the centerline set; local coarse paths are planned for each local section, and then combined into a complete global coarse path; based on the global coarse trajectory, the coarse trajectory is derived as the initial guess for the subsequent optimization problem;

[0110] Module M3: For the problem that the global rough trajectory is collision-free but kinematically infeasible, the kinematic constraints are softened into the objective function as penalty functions, and linear obstacle avoidance constraints are used to construct and solve a simplified optimization problem to obtain an approximate kinematically feasible trajectory;

[0111] Module M4: Based on wide and narrow local road sections and local obstacles, obstacle avoidance constraints are constructed between the vehicle and the local obstacles. By gradually increasing the sampling points in the optimization problem, the optimization problem with a smaller number of sampling points is used to warm-start the subsequent optimization problem with a larger number of sampling points. The optimal trajectory is solved iteratively until the number of sampling points reaches the threshold, and finally the trajectory planning of the rear-wheel steering vehicle in the road scenario is realized.

[0112] Preferably, the module M1 comprises:

[0113] Module M1.1: Kinematic modeling;

[0114] In the fixed planning time domain T, the number of sampling points is N. The first-order Euler discretization method is used to set the rear axle midpoint position (x0, y0), the heading angle θ, and the wheel steering angle of the rear-wheel steering vehicle. Rear wheel steering angle With the rear wheel speed v as the state quantity, set the front wheel steering angle angular velocity ω f 、Rear wheel steering angle angular velocity ω r With the rear wheel acceleration a as the control variable, the vehicle motion formula under discrete time is obtained as follows:

[0115]

[0116] Where Δt is the sampling time Δt = T / N; s(k) = [x(k), y(k), θ(k), φ f (k),φ r (k),v(k)] T is the state quantity at the kth moment, u(k)=[ω f (k),ω r (k),a(k)] T is the control quantity at the kth moment, Δ is the difference of the state / control quantity at adjacent moments; f(s(k),u(k)) is the kinematic equation of the system;

[0117] At the moment k = 0, ..., N, the front and rear wheel steering angles and angular velocities, rear wheel velocities and accelerations of the vehicle are subject to physical constraints, and the maximum / minimum constraints of the design state quantities and control quantities are as follows:

[0118]

[0119] in, v max They are the maximum front wheel turning angle, maximum rear wheel turning angle and maximum rear wheel linear speed of the vehicle, a max They are the maximum front wheel angular velocity, maximum rear wheel angular velocity and maximum rear wheel acceleration of the vehicle respectively;

[0120] Module M1.2: The state of the vehicle at time k = 0 is the current state of the vehicle, and the design boundary condition constraints are as follows:

[0121] s(0)=s start #(3)

[0122] Module M1.3: In road scenes, use convex polygons to model other vehicles and other obstacles. Among them, Obs j is the jth obstacle, is the 1st,…,Nth j Vertices, N j is the number of vertices of the jth obstacle, N obs is the number of obstacles; the shape of the vehicle is approximated by a rectangle, and the vertices of the vehicle at time k are represented as Veh(k): = {A(k), B(k), C(k), D(k)}, where Veh(k) represents the vertex geometry of the vehicle at time k, and A(k), B(k), C(k), and D(k) represent the four vertices of the rectangle covering the vehicle body; at time k = 0, ..., N, the vehicle needs to avoid collision with obstacles, so the obstacle avoidance constraint is expressed as:

[0123]

[0124] OutPoly(a,{b1,…,b n}) function represents the point a in the convex polygon {b1,…,b n}, where n is the number of sides of the polygon;

[0125]

[0126] Among them, S (·) represents the area of ​​a triangle or convex polygon, ∈ is the relaxation parameter, where the area of ​​the convex polygon is obtained by offline calculation. The area calculation formula is:

[0127]

[0128] in, Points a and b i ,b i+1 Coordinate values ​​in the two-dimensional plane;

[0129] Module M1.4: Considering the road scenario, the vehicle needs to approach the center line of the road while approaching the target point. Therefore, the objective function expression is:

[0130]

[0131] Where s = [s(0) T ,s(1) T ,…,s(N)] T Represents the state quantity at all times, u=[u(0) T ,u(1) T ,…,u(N-1)] T represents the control quantity at all times, ω g ,ω e ,ω c are the weights of approaching the end point, approaching the center line of the road, and reducing control consumption; x g is the state quantity of the end point, ||e(k)|| 2 =e p (k) 2 +ω θ e θ (k) 2 is the penalty for deviation from the center line of the road at time k, where e p (k) and e θ (k) represents the lateral error and heading angle error between the midpoint of the vehicle's rear axle and the centerline of the road, ω θ is the weight of the heading angle error; the road centerline is discretized into a set in is the set of midpoints of the road centerline, P cen [k] is the position of the kth point in the set, (x cen [k],y cen [k]) are the positions of the kth point in the set on the two-dimensional plane, θ cen [k] is the heading angle of the kth point in the set, is the number of points in the center line set;

[0132] To derive e p (k) and e θ The analytical expression of (k) is as follows: Let the initial guess of the vehicle position in the optimization problem be Where (xinit (k),y init (k)) is the initial guess of the vehicle position at time k; then, point (x init (k),y init (k)) Projected onto the road centerline, we get a set of reference points in, is (x init (k),y init (k)) The position of the projection point on the road centerline, and its corresponding relationship is: Assume that the center line of the road is composed of straight line segments and circular arc segments, and the projection point of the variable (x(k), y(k)) is ref (k),y ref (k)) are in the same straight line / arc segment. For the straight line segment, calculate e p (k) and e θ The expression of (k) is:

[0133] e p (k) = (y(k) - y re f (k))cosθ ref (k)-(x(k)-x ref (k))sinθ ref (k)

[0134] e θ (k) = θ(k) - θ ref (k)

[0135] For circular arc segments, calculate and The expression is:

[0136]

[0137] e θ (k) = θ(k) - θ ref (k)+θ d (k)

[0138] Among them, (x R (k),y R (k)) is the position of the center of the arc segment, R(k) is the radius of the arc segment corresponding to time k, θ d (k) is the position of the vehicle at time k (x(k), y(k)) and the position of the reference point (x ref (k),y ref (k)) corresponds to the arc center (x R (k),y R (k)) angle difference.

[0139] Preferably, the module M2 comprises:

[0140] Module M2.1: Centerline segmentation and identification of local obstacles;

[0141] Module M2.1.1: Classification of points on the centerline and identification of local obstacles;

[0142] For each point on the centerline, it is divided into two categories: wide and narrow according to its collision risk with surrounding obstacles along the centerline; among them, for the kth point P on the centerline cen [k]=(x cen [k],y cen [k],θ cen [k]), if any vehicle position in a certain neighborhood With any obstacles Obs j ,j=1,...,N obs If no collision occurs, it is considered that there is no collision risk; the calculation standard is:

[0143]

[0144] Among them, Obs is the obstacle set {Obs j ,j=1,...,N obs}, CheckCollisionRisk(P cen ,Obs) is the judgment point P cen Function of the collision risk with obstacle Obs; For point P cen The surrounding area is expressed as: where p thre and θ thre are the thresholds for position and angle respectively;

[0145] If there is no collision risk, then point P is considered cen [k]=(x cen [k],y cen [k],θ cen [k]) is classified as wide, otherwise it is classified as narrow; at the same time, according to the distance between the obstacle and point P cen [k], and identify P cen [k] nearby local obstacles, specifically: if the obstacle Obs j A vertex of cen If the distance between [k] is within the threshold Δs, then Obs j Point P cen [k] is in the surrounding obstacle set LocObs[k];

[0146] Module M2.1.2: Centerline segmentation;

[0147] Judgment point P cen After classification of [k], if it belongs to wide, it is classified into the wide point set, otherwise it is classified into the narrow point set; when the point changes from wide to narrow, the current wide point set is classified into the wide road section set, and the current wide point set is set to the empty set; conversely, when it changes from narrow to wide, the current narrow point set is classified into the narrow road section set, and the current narrow point set is set to the empty set; repeat this operation, traverse the points on the center line, and obtain multiple wide / narrow road sections; among which the i-th road section is represented as Among them, (x[k p ],y[k p ],θ[k p ]) is the kth p The index point, p is the index of the point in the set; N si represents the number of points in the i-th road section;

[0148] Module M2.2: Planning the rough path;

[0149] A local coarse path is planned for each road section. For the i-th road section, if it is a wide road section, the road section itself is used as the coarse path. If it is a narrow road section, the coarse path is planned using the A* algorithm. Then, the road sections are connected and combined in sequence to obtain a global coarse path. Then, the global coarse path is spatially resampled to obtain a coarse path. in is the coarse path, N curr is the number of sampling points, x init [k],y init [k] is the position of the kth path point in the plane coordinate system;

[0150] Module M2.3: Calculate rough trajectory;

[0151] Since the rough trajectory only contains position information, the rough trajectory is calculated based on the obtained rough trajectory What is missing from The expression is as follows:

[0152] θ init (k+1)=tan -1 (Δy init (k),Δx init (k)),k=0,…,N curr -1

[0153] v d (k) = Δx init (k)cosθ init (k)+Δy init(k)sinθ init (k)

[0154] v init (k) = sgn(v d (k))·||(Δx init (k),Δy init (k))|| / Δt c

[0155]

[0156] v init (N curr )=v init (N curr -1)

[0157]

[0158] Among them, x init (k),y i n it (k),θ init (k), v init (k) are respectively x(k), y(k), θ(k), φ at time k f (k),φ r (k), the initial guess of v(k), v d (k) is the speed direction of the vehicle at time k, sgn is the sign function, Δt c =T / N curr The number of sampling points is N curr The sampling time below.

[0159] Preferably, the module M3 comprises:

[0160] Module M3.1: Design penalty functions to soften kinematic constraints;

[0161] For the system kinematic constraint Δs(k)=f(s(k),u(k)), the penalty function is designed as: Add the penalty function to the objective function to obtain a new objective function;

[0162] Module M3.2: Design space-time corridor obstacle avoidance constraints;

[0163] For the kth step in the prediction time domain, the predicted robot position is: (x init (k),y init (k));

[0164] For the predicted robot position, initialize the linear obstacle avoidance constraints:

[0165] x min (k) = x init (k) = x max (k)

[0166] y min (k) = y init (k) = y max (k)

[0167] After that, we can loop to get x as follows: min (k),x max (k),y min (k),y max (k):

[0168] x min (k) = x min (k)-γ

[0169] if C(x min (k)-γ:x max (k),y min (k):y max (k)) = 0

[0170] x max (k) = x max (k)+γ

[0171] if C(x min (k):x max (k)+γ,y min (k):y max [k]) = 0

[0172] y min (k) = y min (k)-γ

[0173] if C(x min (k):x max (k),y min (k)-γ:y max (k)) = 0

[0174] x max (k) = x max (k)+γ

[0175] if C(x min (k):x max (k),y min (k):y max (k)+γ)=0

[0176] Among them, γ is the single step length, C(x min:x max ,y min :y max ) is to convert x min :x max ,y min :y max Function that projects space to a two-dimensional grid map. Its return value is 0 for x min :x max ,y min :y max There are no obstacles in the space, otherwise there are obstacles. The final space-time corridor constraints are as follows:

[0177]

[0178] Indicates that the mobile robot's position is within the local rectangular frame;

[0179] Module M3.3: Solving approximate feasible trajectories;

[0180] By transforming the kinematic constraints into penalty terms in the objective function and constructing the space-time corridor constraints, the optimal control problem is constructed as follows:

[0181]

[0182] st kinematic constraint formula (2),

[0183] Boundary condition constraint formula (3),

[0184] The space-time corridor constraint formula (6) is:

[0185] Among them, s and u are the variables to be solved. is the initial guess, and a nonlinear solver is used to solve the approximate feasible trajectory.

[0186] Preferably, the module M4 comprises:

[0187] Module M4.1: Construct and solve the optimal trajectory under the current sampling point;

[0188] Based on the center line of the road, the obstacle avoidance function is reconstructed, and the initial guess Calculate its projection on the center line For the initial guess point P at time k init (k), if it is classified as wide and is at the projection point P ref (k) Surrounding, i.e. Then the obstacle avoidance constraint is simplified as:

[0189]

[0190] Where P(k) = (x(k), y(k), θ(k)) is the position of the vehicle at time k; otherwise, only the position with respect to the projection point P is considered. ref The obstacle avoidance constraint of the local obstacle Locobs(k) corresponding to (k) is:

[0191]

[0192] Where Δs is the radius of the local area;

[0193] At this point, the optimization problem is constructed as:

[0194] The objective function (5) is:

[0195] st kinematic constraints (1) (2),

[0196] Boundary condition constraint formula (3),

[0197] Simplify the obstacle avoidance constraint formula (7) and formula (8),

[0198] The trajectory obtained in module M3.3 is the initial guess, and a nonlinear solver is used to solve the optimal trajectory at the current sampling point.

[0199] Module M4.2: Iteratively increase sampling points;

[0200] The optimal trajectory of more sampling points is gradually solved by iteratively increasing the sampling points. In each iteration, the current sampling points are increased to α times the original number, and the optimal trajectory under the original number of sampling points is resampled to the trajectory of the current number of sampling points according to time. This is used as the initial solution. According to the method in module M4.1, the optimal trajectory under the current sampling point is calculated, and this cycle is repeated until the current number of sampling points N curr Reaching the threshold sampling point N thre .

[0201] Compared with the prior art, the present invention has the following beneficial effects:

[0202] (1) The present invention derives the formula of the vehicle position error from the center line and the heading angle error, and uses it and the smoothness index as components of the objective function. During the optimization process, as the objective function is minimized, a smooth trajectory close to the center line can be planned;

[0203] (2) The present invention adopts a method of iteratively increasing sampling points, starting from a simple optimization problem (with a small number of sampling points), and providing a warm start for a complex optimization problem (with a large number of sampling points) by continuously increasing the number of sampling points, thereby accelerating the speed of solving the optimization problem, and can achieve the effect of improving planning efficiency and saving computing power;

[0204] (3) The present invention is combined with a vehicle positioning algorithm to realize trajectory planning of a rear-wheel steering vehicle in a road scene, and is robust to the scene and has a high-quality planned trajectory. BRIEF DESCRIPTION OF THE DRAWINGS

[0205] Other features, objects and advantages of the present invention will become more apparent from the detailed description of non-limiting embodiments made with reference to the following drawings:

[0206] Figure 1 It is a flow chart of the present invention;

[0207] Figure 2 This is a diagram illustrating the calculation method for points outside the polygon;

[0208] Figure 3 This is a diagram illustrating the calculation method of the lateral error and heading angle error of a straight line segment;

[0209] Figure 4 The figure illustrates the calculation method of the lateral error and heading angle error of the arc segment. DETAILED DESCRIPTION

[0210] The present invention is described in detail below in conjunction with specific embodiments. The following embodiments will help those skilled in the art to further understand the present invention, but are not intended to limit the present invention in any form. It should be noted that, for those of ordinary skill in the art, several changes and improvements can also be made without departing from the concept of the present invention. These all belong to the protection scope of the present invention.

[0211] Example 1

[0212] Based on previous work, the present invention innovatively proposes a rear-wheel steering vehicle trajectory planning method in road scenarios based on iterative optimization. The algorithm can be applied to the path planning tasks of rear-wheel steering vehicles in road scenarios.

[0213] The present invention provides a rear-wheel steering vehicle trajectory planning method in a road scene based on iterative optimization. The method first establishes an optimization problem, derives the kinematic formula of the rear-wheel steering vehicle, the obstacle avoidance constraint formula and the calculation formula of the distance and angle error between the center point of the rear axle and the center line of the road; then, an initial guess is generated, and based on the road scene structure, a collision-free trajectory that fits the center line of the road as closely as possible is constructed as the initial guess in the optimization problem, and the subsequent optimization problem is warm-started; then, an approximate feasible trajectory is obtained, the kinematic constraint is softened into a penalty function and a space-time corridor constraint is constructed, and the optimization problem is constructed and solved to obtain an approximate kinematic feasible trajectory; finally, the optimal trajectory is solved, and the sampling points in the optimization problem are gradually increased, and the optimization problem with a small number of sampling points is used to warm-start the subsequent optimization problem with a large number of sampling points until the number of sampling points reaches a threshold. The present invention is combined with a vehicle positioning algorithm to realize the trajectory planning of a rear-wheel steering vehicle in a road scene, and at the same time, it is robust to the scene and the planned trajectory is of high quality.

[0214] In view of the shortcomings of the original technology, this paper innovatively proposes a rear-wheel steering vehicle trajectory planning method based on iterative optimization in road scenarios. In the specific implementation process, the method process is as follows: Figure 1 shown.

[0215] The rear-wheel steering vehicle trajectory planning method in a road scene based on iterative optimization proposed by the present invention mainly includes the following steps:

[0216] Step 1: Establish the optimization problem. First, establish the kinematic model of the rear-wheel steering vehicle. Based on the geometric relationship in the road scene, establish the analytical expression of the distance and angle error between the midpoint of the vehicle's rear axle and the center line of the road. Formulate the trajectory planning problem as an optimization problem.

[0217] Step 2: Generate initial guesses. Based on the collision risk between the centerline and surrounding obstacles, divide the road centerline into several "wide" and "narrow" local sections, and find the corresponding local obstacles for each point in the centerline set. Plan local coarse paths for each local section and combine them into a complete global coarse path. Based on the global coarse trajectory, derive the coarse trajectory as the initial guess for the subsequent optimization problem.

[0218] Step 3: Obtain an approximate feasible trajectory. For the problem that the global rough trajectory is collision-free but kinematically infeasible, the kinematic constraint is softened into the objective function as a penalty function, and a linear obstacle avoidance constraint is used to construct and solve a simplified optimization problem to obtain an approximate kinematically feasible trajectory.

[0219] Step 4: Obtain the optimal trajectory. Based on the "wide" and "narrow" local road sections and local obstacles in step 2, construct obstacle avoidance constraints between the vehicle and the local obstacles. By gradually increasing the sampling points in the optimization problem, the optimization problem with a smaller number of sampling points is used to warm-start the subsequent optimization problem with a larger number of sampling points, and the optimal trajectory is iteratively solved until the number of sampling points reaches the threshold. Finally, the trajectory planning of the rear-wheel steering vehicle in the road scene is realized.

[0220] Specifically, the modeling process of step 1 mainly includes the following steps:

[0221] Step 1.1: Kinematic modeling. Consider an optimization problem with N sampling points in a fixed planning time domain (T). Use the first-order Euler discretization method to set the rear axle midpoint position (x, y), heading angle (θ), and wheel steering angle of the rear-wheel steering vehicle. Rear wheel steering angle With the rear wheel speed (v) as the state quantity, set the front wheel steering angle velocity (ω f ), rear wheel steering angle angular velocity (ω r ) and rear wheel acceleration (a) as control variables, the vehicle motion formula under discrete time is obtained as follows:

[0222]

[0223] Where Δt is the sampling time Δt = T / N, is the state quantity at the kth moment, u(k)=[ω f (k),ω r (k),a(k)] T is the control quantity at the kth moment, Δ is the difference of the state / control quantity at adjacent moments (taking Δx(k)=x(k+1)-x(k) as an example), and f(s(k),u(k)) is the kinematic equation of the system.

[0224] Considering the actual situation, at the moment k = 0, ..., N, the front and rear wheel steering angles and angular velocities, rear wheel velocities and accelerations of the vehicle are subject to physical constraints, and the maximum (minimum) value constraints of the design state quantity and control quantity are as follows:

[0225]

[0226] in, v max They are the maximum front wheel turning angle, maximum rear wheel turning angle and maximum rear wheel linear speed of the vehicle, a max They are the vehicle's maximum front wheel angular velocity, maximum rear wheel angular velocity and maximum rear wheel acceleration respectively.

[0227] Step 1.2: Boundary condition constraints. Considering the optimization problem, the state of the vehicle at time k = 0 is the current state of the vehicle, and the boundary condition constraints are designed as follows:

[0228] s(0)=s start #(3)

[0229] Step 1.3: Obstacle avoidance constraints. In the road scene, use convex polygons to model other vehicles and other obstacles as Among them, Obs j is the jth obstacle, is the 1st,…,Nth j Vertices, N j is the number of vertices of the jth obstacle, N obs is the number of obstacles. The shape of the vehicle is approximated by a rectangle, and the vertices of the vehicle at time k are represented as Veh(k): = {A(k), B(k), C(k), D(k)}, where Veh(k) represents the vertex geometry of the vehicle at time k, and A(k), B(k), C(k), and D(k) represent the four vertices of the rectangle covering the vehicle body. At time k = 0, ..., N, the vehicle needs to avoid collision with obstacles, so the obstacle avoidance constraint can be expressed as:

[0230]

[0231] Among them, OutPoly(a,{b1,…,b n}) function represents the point a in the convex polygon {b1,…,b n} external, such as Figure 2 As shown, its expression is:

[0232]

[0233] Among them, S (·) represents the area of ​​a triangle or convex polygon, and ∈ is a slack variable. The area of ​​a convex polygon can be calculated offline, while the area of ​​a triangle For example, the area calculation formula is:

[0234]

[0235] in, Points a and b i ,b i+1 Coordinate values ​​in a two-dimensional plane.

[0236] Step 1.4: Objective function. Considering the road scenario, the vehicle needs to approach the target point while staying as close to the center line of the road as possible, while reducing energy consumption and improving passenger comfort. Therefore, its expression is:

[0237]

[0238] Where s = [s(0) T ,s(1) T ,…,s(N)] T Represents the state quantity at all times, u=[u(0) T ,u(1) T ,…,u(N-1)] T represents the control quantity at all times, ω g ,ω e ,ω c are the weights of approaching the end point, approaching the center line of the road, and reducing control consumption. g is the state quantity of the end point, ||e(k)|| 2 =e p (k) 2 +ω θ e θ (k) 2 is the penalty for deviation from the center line of the road at time k, where e θ (k) and e θ (k) represents the lateral error and heading angle error between the midpoint of the vehicle's rear axle and the centerline of the road, ω θ is the weight of the heading angle error. In more detail, the road centerline is discretized into a set in is the set of midpoints of the road centerline, P cen [k] is the position of the kth point in the set, (x cen [k],y cen [k]) are the positions of the kth point in the set on the two-dimensional plane θ cen [k] is the heading angle of the kth point in the set, is the number of points in the centerline set.

[0239] To derive e p (k) and e θ The analytical expression of (k) is as follows: Let the initial guess of the vehicle position in the optimization problem be Where (x init (k),y init (k)) is the initial guess of the vehicle position at time k. Then, point (x init (k),y init (k)) Projected onto the road centerline, we get a set of reference points in, is (x init (k),y init (k)) The position of the projection point on the road centerline, and its corresponding relationship is: It is assumed that the road centerline consists of straight line segments and circular arc segments, and the projection point of the variable (x(k), y(k)) is ref (k),y ref (k)) are in the same straight line / arc segment. For a straight line segment, if Figure 3 As shown, calculate e p (k) and e θ The expression of (k) is:

[0240] e p (k) = (y(k) - y ref (k))cosθ ref (k)-(x(k)-x ref (k))sinθ ref (k),

[0241] e θ (k) = θ(k) - θ ref (k).

[0242] For arc segments, such as Figure 4 As shown, calculation and The expression is:

[0243]

[0244] e θ (k) = θ(k) - θ ref (k)+θ d (k),

[0245] Among them, (x R (k),y R (k)) is the position of the center of the arc segment, R(k) is the radius of the arc segment corresponding to time k, θ d (k) is the position of the vehicle at time k (x(k), y(k)) and the position of the reference point (x ref (k),y ref (k)) corresponds to the arc center (x R (k),y R (k)) angle difference.

[0246] Specifically, the approximation process of step 2 mainly includes the following steps:

[0247] Step 2.1: Segment the centerline and identify local obstacles.

[0248] Step 2.1.1: Classification of points on the centerline and identification of local obstacles. For each point on the centerline, classify it into two categories: "wide" or "narrow" according to its collision risk with surrounding obstacles. cen[k]=(x cen [k],y cen [k],θ cen [k]), if any vehicle position in a certain neighborhood With any obstacles Obs j ,j=1,...,N obs If no collision occurs, it is considered that there is no collision risk. The calculation standard is:

[0249]

[0250] Among them, Obs is the obstacle set {Obs j ,j=1,...,N obs}, CheckCollisionRisk(P cen ,Obs) is the judgment point P cen Function of the collision risk with the obstacle Obs, For point P cen The surrounding area is expressed as: where p thre and θ thre are the thresholds for position and angle respectively.

[0251] If there is no collision risk, then point P is considered cen [k]=(x cen [k],y cen [k],θ cen [k]) is classified as “wide”, otherwise it is classified as “narrow”. cen [k], and identify P cen [k] nearby local obstacles, the specific identification method is: if the obstacle Obs j A vertex of cen The distance between [k] is within the threshold Δs, that is, Obs j Point P cen [k] is in the surrounding obstacle set LocObs[k].

[0252] Step 2.1.2: Centerline segmentation. Determine point P cenAfter classification of [k], if it belongs to "wide", it will be included in the "wide" point set, otherwise it will be included in the "narrow" point set. When the point changes from "wide" to "narrow", the current "wide" point set is included in the "wide" road section set, and the current "wide" point set is set to the empty set. Conversely, when it changes from "narrow" to "wide", the current "narrow" point set is included in the "narrow" road section set, and the current "narrow" point set is set to the empty set. Repeat this operation, traverse the points on the center line, and obtain several "wide" / "narrow" road sections. Among them, the i-th road section can be expressed as Among them, (x[k p ],y[k p ],θ[k p ]) is the kth p The index point, p is the index of the point in the set; N si Represents the number of points in the i-th road segment.

[0253] Step 2.2: Plan a coarse path. Plan a local coarse path for each road segment. For the i-th road segment, if it is a "wide" road segment, the road segment itself is used as the coarse path. If it is a "narrow" road segment, the coarse path is planned using the A* algorithm. Then, the road segments are connected and combined in sequence to obtain a global coarse path. Then, the global coarse path is spatially resampled to obtain a coarse path. in is the coarse path, N curr is the number of sampling points, x init [k],y init [k] is the position of the kth path point in the plane coordinate system.

[0254] Step 2.3: Calculate the rough trajectory. Since the rough trajectory only contains position information, the rough trajectory is calculated based on the obtained rough trajectory. What is missing from as follows:

[0255] θ init (k+1)=tan -1 (Δy init (k),Δx init (k)),k=0,…,N curr -1,

[0256] v d (k) = Δx init (k)cosθ init (k)+Δy init (k)sinθ init (k),

[0257] v init (k) = sgn(v d(k))·||(Δx init (k),Δy init (k))|| / Δt c ,

[0258]

[0259] v init (N curr )=v init (N curr -1),

[0260]

[0261] Among them, x init (k),y init (k),θ init (k), v init (k) are respectively x(k), y(k), θ(k), The initial guess of v(k), v d (k) is the speed direction of the vehicle at time k, sgn is the sign function, Δt c =T / N curr The number of sampling points is N curr The sampling time below.

[0262] Specifically, the planning process of step 3 mainly includes the following steps:

[0263] Step 3.1: Design a penalty function. Since the kinematic constraints of the system bring inconvenience to the solution, a penalty function is designed to soften the kinematic constraints. For the system kinematic constraint Δs(k) = f(s(k), u(k)), the penalty function is designed as: Add the penalty function to the objective function to obtain a new objective function.

[0264] Step 3.2: Design space-time corridor obstacle avoidance constraints. For the kth step in the prediction time domain, the predicted robot position is: (x init (k),y init (k)). For the predicted robot position, initialize the linear obstacle avoidance constraint:

[0265] x min (k) = x i n it (k) = x max (k),

[0266] y min (k) = y init (k) = y max (k)

[0267] After that, we can loop to get x as follows: min (k),x max (k),y min (k),y max (k):

[0268] x min (k) = x min (k)-γ

[0269] if C(x min (k)-γ:x max (k),y mim (k):y max (k)) = 0

[0270] x max (k) = x max (k)+γ

[0271] if C(x min (k):x max (k)+γ,y min (k):y max [k]) = 0

[0272] y min (k) = y min (k)-γ

[0273] if C(x min (k):x max (k),y min (k)-γ:y max (k)) = 0

[0274] x max (k) = x max (k)+γ

[0275] if C(x min (k):x mmx (k),y min (k):y max (k)+γ)=0

[0276] Among them, γ is the single step length, C(x min :x max ,y min :y max ) is to convert x min :x max ,y min :y max Function that projects space to a two-dimensional grid map. Its return value is 0 for x min :x max ,y min :ymax There are no obstacles in the space, otherwise there are obstacles. The final space-time corridor constraints are as follows:

[0277]

[0278] Represents the position of the mobile robot within the local rectangular box.

[0279] Step 3.3: Solve the approximate feasible trajectory. By converting the kinematic constraints into penalty terms in the objective function and constructing the space-time corridor constraints, the optimal control problem is constructed as follows:

[0280]

[0281] st kinematic constraints (2),

[0282] Boundary condition constraints (3),

[0283] Space-time corridor constraints (6),

[0284] Among them, s and u are the variables to be solved. is the initial guess, and a nonlinear solver is used to solve the approximate feasible trajectory.

[0285] Specifically, the tracking process of step 4 mainly includes the following steps:

[0286] Step 4.1: Construct and solve the optimal trajectory under the current sampling point. Since the obstacle avoidance constraint (4) contains the obstacle avoidance constraints for all obstacles, it poses a great challenge to solve the optimization problem. Therefore, the obstacle avoidance function is reconstructed based on the center line of the road. Calculate its projection on the center line For the initial guess point P at time k init (k) If it is classified as “wide” and is at the projection point P ref (k) around, i.e. Then the obstacle avoidance constraint is simplified as:

[0287]

[0288] Among them, P(k) = (x(k), y(k), θ(k)) is the position of the vehicle at time k. Otherwise, only the position with respect to the projection point P is considered. ref The obstacle avoidance constraint of the local obstacle Locobs(k) corresponding to (k) is:

[0289]

[0290] Among them, Δs is the radius of the local area. So far, the optimization problem can be constructed as:

[0291] Objective function (5),

[0292] st kinematic constraints (1)(2),

[0293] Boundary condition constraints (3),

[0294] Simplify obstacle avoidance constraints (7)(8),

[0295] Take the trajectory obtained in step 3.3 is the initial guess, and a nonlinear solver is used to solve the optimal trajectory at the current sampling point.

[0296] Step 4.2: Iteratively increase sampling points. Since there are currently few sampling points, in order to ensure the smoothness of the trajectory, the trajectory is required to include at least the threshold number N thre Therefore, the optimal trajectory of more sampling points is gradually solved by iteratively increasing the sampling points. In each iteration, the current sampling points are increased to α times the original ones, that is, N←α·N.

[0297] After curr curr, the optimal trajectory under the original number of sampling points is resampled according to time to the trajectory of the current number of sampling points. This is used as the initial solution. According to the method in step S4.1, the optimal trajectory under the current sampling point is calculated, and this cycle is repeated until the current number of sampling points N curr Reaching the threshold sampling point N thre .

[0298] Example 2

[0299] The present invention also provides a rear-wheel steering vehicle trajectory planning system under road scenarios based on iterative optimization. The rear-wheel steering vehicle trajectory planning system under road scenarios based on iterative optimization can be implemented by executing the process steps of the rear-wheel steering vehicle trajectory planning method under road scenarios based on iterative optimization, that is, those skilled in the art can understand the rear-wheel steering vehicle trajectory planning method under road scenarios based on iterative optimization as a preferred implementation of the rear-wheel steering vehicle trajectory planning system under road scenarios based on iterative optimization.

[0300] The rear-wheel steering vehicle trajectory planning system in a road scene based on iterative optimization provided by the present invention includes:

[0301] Module M1: Establish a kinematic model for a rear-wheel steering vehicle. Based on the geometric relationship in the road scene, establish an analytical expression for the distance and angle error between the midpoint of the vehicle's rear axle and the centerline of the road, and formulate the trajectory planning problem as an optimization problem.

[0302] Module M2: Based on the collision risk between the centerline and surrounding obstacles, the road centerline is divided into multiple wide and narrow local sections, and the corresponding local obstacles are found for each point in the centerline set; local coarse paths are planned for each local section, and then combined into a complete global coarse path; based on the global coarse trajectory, the coarse trajectory is derived as the initial guess for the subsequent optimization problem;

[0303] Module M3: For the problem that the global rough trajectory is collision-free but kinematically infeasible, the kinematic constraints are softened into the objective function as penalty functions, and linear obstacle avoidance constraints are used to construct and solve a simplified optimization problem to obtain an approximate kinematically feasible trajectory;

[0304] Module M4: Based on wide and narrow local road sections and local obstacles, obstacle avoidance constraints are constructed between the vehicle and the local obstacles. By gradually increasing the sampling points in the optimization problem, the optimization problem with a smaller number of sampling points is used to warm-start the subsequent optimization problem with a larger number of sampling points. The optimal trajectory is solved iteratively until the number of sampling points reaches the threshold, and finally the trajectory planning of the rear-wheel steering vehicle in the road scenario is realized.

[0305] The module M1 comprises:

[0306] Module M1.1: Kinematic modeling;

[0307] In the fixed planning time domain T, the number of sampling points is N. The first-order Euler discretization method is used to set the rear axle midpoint position (x0, y0), the heading angle θ, and the wheel steering angle of the rear-wheel steering vehicle. Rear wheel steering angle With the rear wheel speed v as the state quantity, set the front wheel steering angle angular velocity ω f 、Rear wheel steering angle angular velocity ω r With the rear wheel acceleration a as the control variable, the vehicle motion formula under discrete time is obtained as follows:

[0308]

[0309] Where Δt is the sampling time Δt = T / N; s(k) = [x(k), y(k), θ(k), φ f (k),φ r (k),v(k)] T is the state quantity at the kth moment, u(k)=[ω f (k),ω r (k),a(k)] T is the control quantity at the kth moment, Δ is the difference of the state / control quantity at adjacent moments; f(s(k),u(k)) is the kinematic equation of the system;

[0310] At the moment k = 0, ..., N, the front and rear wheel steering angles and angular velocities, rear wheel velocities and accelerations of the vehicle are subject to physical constraints, and the maximum / minimum constraints of the design state quantities and control quantities are as follows:

[0311]

[0312] in, v max They are the maximum front wheel turning angle, maximum rear wheel turning angle and maximum rear wheel linear speed of the vehicle, a max They are the maximum front wheel angular velocity, maximum rear wheel angular velocity and maximum rear wheel acceleration of the vehicle respectively;

[0313] Module M1.2: The state of the vehicle at time k = 0 is the current state of the vehicle, and the design boundary condition constraints are as follows:

[0314] s(0)=s start #(3)

[0315] Module M1.3: In road scenes, use convex polygons to model other vehicles and other obstacles. Among them, Obs j is the jth obstacle, is the 1st,…,Nth j Vertices, N j is the number of vertices of the jth obstacle, N obs is the number of obstacles; the shape of the vehicle is approximated by a rectangle, and the vertices of the vehicle at time k are represented as Veh(k): = {A(k), B(k), C(k), D(k)}, where Veh(k) represents the vertex geometry of the vehicle at time k, and A(k), B(k), C(k), and D(k) represent the four vertices of the rectangle covering the vehicle body; at time k = 0, ..., N, the vehicle needs to avoid collision with obstacles, so the obstacle avoidance constraint is expressed as:

[0316]

[0317] OutPoly(a,{b1,…,b n}) function represents the point a in the convex polygon {b1,…,b n}, where n is the number of sides of the polygon;

[0318]

[0319] Among them, S (·) represents the area of ​​a triangle or convex polygon, ∈ is the relaxation parameter, where the area of ​​the convex polygon is calculated offline. The area calculation formula is:

[0320]

[0321] in, Points a and b i ,b i+1 Coordinate values ​​in the two-dimensional plane;

[0322] Module M1.4: Considering the road scenario, the vehicle needs to approach the center line of the road while approaching the target point. Therefore, the objective function expression is:

[0323]

[0324] Where s = [s(0) T ,s(1) T ,…,s(N)] T Represents the state quantity at all times, u=[u(0) T ,u(1) T ,…,u(N-1)] T represents the control quantity at all times, ω g ,ω e ,ω c are the weights of approaching the end point, approaching the center line of the road, and reducing control consumption; x g is the state quantity of the end point, ||e(k)|| 2 =e p (k) 2 +ω θ e θ (k) 2 is the penalty for deviation from the center line of the road at time k, where e p (k) and e θ (k) represents the lateral error and heading angle error between the midpoint of the vehicle's rear axle and the centerline of the road, ω θ is the weight of the heading angle error; the road centerline is discretized into a set in is the set of midpoints of the road centerline, P cen [k] is the position of the kth point in the set, (x cen [k],y cen [k]) are the positions of the kth point in the set on the two-dimensional plane, θ cen [k] is the heading angle of the kth point in the set, is the number of points in the center line set;

[0325] To derive e p (k) and e θ The analytical expression of (k) is as follows: Let the initial guess of the vehicle position in the optimization problem be Where (xinit (k),y init (k)) is the initial guess of the vehicle position at time k; then, point (x init (k),y init (k)) Projected onto the road centerline, we get a set of reference points in, is (x init (k),y init (k)) The position of the projection point on the road centerline, and its corresponding relationship is: Assume that the center line of the road is composed of straight line segments and circular arc segments, and the projection point of the variable (x(k), y(k)) is ref (k),y ref (k)) are in the same straight line / arc segment. For the straight line segment, calculate e p (k) and e θ The expression of (k) is:

[0326] e p (k) = (y(k) - y re f (k))cosθ ref (k)-(x(k)-x ref (k))sinθ ref (k)

[0327] e θ (k) = θ(k) - θ ref (k)

[0328] For circular arc segments, calculate e pi (k) and e θi The expression of (k) is:

[0329]

[0330] e θ (k) = θ(k) - θ ref (k)+θ d (k)

[0331] Among them, (x R (k),y R (k)) is the position of the center of the arc segment, R(k) is the radius of the arc segment corresponding to time k, θ d (k) is the position of the vehicle at time k (x(k), y(k)) and the position of the reference point (x ref (k),y ref (k)) corresponds to the arc center (x R (k),y R (k)) angle difference.

[0332] The module M2 comprises:

[0333] Module M2.1: Centerline segmentation and identification of local obstacles;

[0334] Module M2.1.1: Classification of points on the centerline and identification of local obstacles;

[0335] For each point on the centerline, it is divided into two categories: wide and narrow according to its collision risk with surrounding obstacles along the centerline; among them, for the kth point P on the centerline cen [k]=(x cen [k],y cen [k],θ cen [k]), if any vehicle position in a certain neighborhood With any obstacles Obs j ,j=1,...,N obs If no collision occurs, it is considered that there is no collision risk; the calculation standard is:

[0336]

[0337] Among them, Obs is the obstacle set {Obs j ,j=1,...,N obs}, CheckCollisionRisk(P cen ,Obs) is the judgment point P cen Function of the collision risk with the obstacle Obs; For point P cen The surrounding area is expressed as: where p thre and θ thre are the thresholds for position and angle respectively;

[0338] If there is no collision risk, then point P is considered cen [k]=(x cen [k],y cen [k],θ cen [k]) is classified as wide, otherwise it is classified as narrow; at the same time, according to the distance between the obstacle and point P cen [k], and identify P cen [k] nearby local obstacles, specifically: if the obstacle Obs j A vertex of cen If the distance between [k] is within the threshold Δs, then Obs j Point P cen [k] is in the surrounding obstacle set LocObs[k];

[0339] Module M2.1.2: Centerline segmentation;

[0340] Judgment point P cen After classification of [k], if it belongs to wide, it is classified into the wide point set, otherwise it is classified into the narrow point set; when the point changes from wide to narrow, the current wide point set is classified into the wide road section set, and the current wide point set is set to the empty set; conversely, when it changes from narrow to wide, the current narrow point set is classified into the narrow road section set, and the current narrow point set is set to the empty set; repeat this operation, traverse the points on the center line, and obtain multiple wide / narrow road sections; among which the i-th road section is represented as Among them, (x[k p ],y[k p ],θ[k p ]) is the kth p The indexed point, p is the index of the point in the set; represents the number of points in the i-th road section;

[0341] Module M2.2: Planning the rough path;

[0342] A local coarse path is planned for each road section. For the i-th road section, if it is a wide road section, the road section itself is used as the coarse path. If it is a narrow road section, the coarse path is planned using the A* algorithm. Then, the road sections are connected and combined in sequence to obtain a global coarse path. Then, the global coarse path is spatially resampled to obtain a coarse path. in is the coarse path, N curr is the number of sampling points, x init [k],y init [k] is the position of the kth path point in the plane coordinate system;

[0343] Module M2.3: Calculate rough trajectory;

[0344] Since the rough trajectory only contains position information, the rough trajectory is calculated based on the obtained rough trajectory What is missing from The expression is as follows:

[0345] θ init (k+1)=tan -1 (Δy init (k),Δx init (k)),k=0,…,N curr -1

[0346] v d (k) = Δx init (k)cosθ int (k)+Δy init (k)sinθ init(k)

[0347] v init (k) = sgn(v d (k))·||(Δx init (k),Δy init (k))|| / Δt c

[0348]

[0349] v init (N curr )=v init (N curr -1)

[0350]

[0351] Among them, x init (k),y init (k),θ init (k), v init (k) are respectively x(k), y(k), θ(k), φ at time k f (k),φ r (k), the initial guess of v(k), v d (k) is the speed direction of the vehicle at time k, sgn is the sign function, Δt c =T / N curr The number of sampling points is N curr The sampling time below.

[0352] The module M3 comprises:

[0353] Module M3.1: Design penalty functions to soften kinematic constraints;

[0354] For the system kinematic constraint Δs(k)=f(s(k),u(k)), the penalty function is designed as: Add the penalty function to the objective function to obtain a new objective function;

[0355] Module M3.2: Design space-time corridor obstacle avoidance constraints;

[0356] For the kth step in the prediction time domain, the predicted robot position is: (x init (k),y init (k));

[0357] For the predicted robot position, initialize the linear obstacle avoidance constraints:

[0358] x min (k) = x init(k) = x max (k)

[0359] y min (k) = y init (k) = y max (k)

[0360] After that, we can loop to get x as follows: min (k),x max (k),y min (k),y max (k):

[0361] x min (k) = x min (k)-γ

[0362] if C(x min (k)-γ:x max (k),y min (k):y max (k)) = 0

[0363] x max (k) = x max (k)+γ

[0364] if C(x min (k):x max (k)+γ,y min (k):y max [k]) = 0

[0365] y min (k) = y min (k)-γ

[0366] if C(x min (k):x max (k),y min (k)-γ:y max (k)) = 0

[0367] x max (k) = x max (k)+γ

[0368] if C(x min (k):x max (k),y min (k):y max (k)+γ)=0

[0369] Among them, γ is the single step length, C(x min :x max ,y min :y max ) is to convert xmin :x max ,y min :y max Function that projects space to a two-dimensional grid map. Its return value is 0 for x min :x max ,y min :y max There are no obstacles in the space, otherwise there are obstacles. The final space-time corridor constraints are as follows:

[0370]

[0371] Indicates that the mobile robot's position is within the local rectangular frame;

[0372] Module M3.3: Solving approximate feasible trajectories;

[0373] By transforming the kinematic constraints into penalty terms in the objective function and constructing the space-time corridor constraints, the optimal control problem is constructed as follows:

[0374]

[0375] st kinematic constraint formula (2),

[0376] Boundary condition constraint formula (3),

[0377] The space-time corridor constraint formula (6) is:

[0378] Among them, s and u are the variables to be solved. is the initial guess, and a nonlinear solver is used to solve the approximate feasible trajectory.

[0379] The module M4 comprises:

[0380] Module M4.1: Construct and solve the optimal trajectory under the current sampling point;

[0381] Based on the center line of the road, the obstacle avoidance function is reconstructed, and the initial guess Calculate its projection on the center line For the initial guess point P at time k init (k), if it is classified as wide and is at the projection point P ref (k) Surrounding, i.e. Then the obstacle avoidance constraint is simplified as:

[0382]

[0383] Where P(k) = (x(k), y(k), θ(k)) is the position of the vehicle at time k; otherwise, only the position with respect to the projection point P is considered. refThe obstacle avoidance constraint of the local obstacle Locobs(k) corresponding to (k) is:

[0384]

[0385] Where Δs is the radius of the local area;

[0386] At this point, the optimization problem is constructed as:

[0387] The objective function (5) is:

[0388] st kinematic constraints (1) (2),

[0389] Boundary condition constraint formula (3),

[0390] Simplify the obstacle avoidance constraint formula (7) and formula (8),

[0391] The trajectory obtained in module M3.3 is the initial guess, and a nonlinear solver is used to solve the optimal trajectory at the current sampling point.

[0392] Module M4.2: Iteratively increase sampling points;

[0393] The optimal trajectory of more sampling points is gradually solved by iteratively increasing the sampling points. In each iteration, the current sampling points are increased to α times the original number, and the optimal trajectory under the original number of sampling points is resampled to the trajectory of the current number of sampling points according to time. This is used as the initial solution. According to the method in module M4.1, the optimal trajectory under the current sampling point is calculated, and this cycle is repeated until the current number of sampling points N curr Reaching the threshold sampling point N thre .

[0394] Those skilled in the art know that, in addition to implementing the system, device and its various modules provided by the present invention in a purely computer-readable program code, it is entirely possible to implement the same program in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers and embedded microcontrollers by logically programming the method steps. Therefore, the system, device and its various modules provided by the present invention can be considered as a hardware component, and the modules included therein for implementing various programs can also be considered as structures within the hardware component; the modules for implementing various functions can also be considered as both software programs for implementing the method and structures within the hardware component.

[0395] The above describes the specific embodiments of the present invention. It should be understood that the present invention is not limited to the above specific embodiments, and those skilled in the art can make various changes or modifications within the scope of the claims, which does not affect the essence of the present invention. In the absence of conflict, the embodiments of the present application and the features in the embodiments can be combined with each other arbitrarily.

Claims

1. A rear-wheel steering vehicle trajectory planning method in a road scene based on iterative optimization, characterized in that: include: Step S1: Establish a kinematic model of a rear-wheel steering vehicle, establish an analytical expression for the distance and angle error between the midpoint of the vehicle's rear axle and the centerline of the road based on the geometric relationship in the road scene, and formulate the trajectory planning problem as an optimization problem; Step S2: Based on the collision risk between the centerline and surrounding obstacles, the centerline of the road is divided into multiple wide and narrow local sections, and at the same time, for each point in the centerline set, the corresponding local obstacle is found; a local coarse path is planned for each local section, and then combined into a complete global coarse path; Based on the global rough trajectory, the rough trajectory is derived as the initial guess for the subsequent optimization problem; Step S3: For the problem that the global rough trajectory is collision-free but kinematically infeasible, the kinematic constraint is softened into the objective function as a penalty function, and a linear obstacle avoidance constraint is used to construct and solve a simplified optimization problem to obtain an approximate kinematically feasible trajectory; Step S4: Based on wide and narrow local road sections and local obstacles, obstacle avoidance constraints are constructed between the vehicle and the local obstacles. By gradually increasing the sampling points in the optimization problem, the optimization problem with a smaller number of sampling points is used to warm-start the subsequent optimization problem with a larger number of sampling points, and the optimal trajectory is iteratively solved until the number of sampling points reaches a threshold, thereby finally realizing the trajectory planning of the rear-wheel steering vehicle in the road scene.

2. The rear-wheel steering vehicle trajectory planning method based on iterative optimization in road scenes according to claim 1 is characterized in that: The step S1 comprises: Step S1.1: kinematic modeling; In the fixed planning time domain T, the number of sampling points is N. The first-order Euler discretization method is used to set the rear axle midpoint position (x0, y0), the heading angle θ, and the wheel steering angle of the rear-wheel steering vehicle. Rear wheel steering angle With the rear wheel speed v as the state quantity, set the front wheel steering angle angular velocity ω f 、Rear wheel steering angle angular velocity ω r With the rear wheel acceleration a as the control variable, the vehicle motion formula under discrete time is obtained as follows: Where Δt is the sampling time Δt = T / N; s(k) = [x(k), y(k), θ(k), φ f (k),φ r (k),v(k)] T is the state quantity at the kth moment, u(k)=[ω f (k),ω r (k),a(k)] T is the control quantity at the kth moment, Δ is the difference of the state / control quantity at adjacent moments; f(s(k),u(k)) is the kinematic equation of the system; At the moment k = 0, ..., N, the front and rear wheel steering angles and angular velocities, rear wheel velocities and accelerations of the vehicle are subject to physical constraints, and the maximum / minimum constraints of the design state quantities and control quantities are as follows: in, v max They are the maximum front wheel turning angle, maximum rear wheel turning angle and maximum rear wheel linear speed of the vehicle, a max They are the maximum front wheel angular velocity, maximum rear wheel angular velocity and maximum rear wheel acceleration of the vehicle respectively; Step S1.2: The state of the vehicle at time k=0 is the current state of the vehicle, and the boundary condition constraints are designed as follows: s(0)=s start #(3) Step S1.3: In the road scene, use convex polygons to model other vehicles and other obstacles as Among them, Obs j is the jth obstacle, is the 1st,…,Nth j Vertices, N j is the number of vertices of the jth obstacle, N obs is the number of obstacles; the shape of the vehicle is approximated by a rectangle, and the vertices of the vehicle at time k are represented as Veh(k): = {A(k), B(k), C(k), D(k)}, where Veh(k) represents the vertex geometry of the vehicle at time k, and A(k), B(k), C(k), and D(k) represent the four vertices of the rectangle covering the vehicle body; at time k = 0, ..., N, the vehicle needs to avoid collision with obstacles, so the obstacle avoidance constraint is expressed as: OutPoly(a,{b1,…,b n }) function represents the point a in the convex polygon {b1,…,b n }, where n is the number of sides of the polygon; Among them, S (·) represents the area of ​​a triangle or convex polygon, ∈ is the relaxation parameter, where the area of ​​the convex polygon is calculated offline. The area calculation formula is: Among them, (x a ,y a ), Points a and b i ,b i+1 Coordinate values ​​in the two-dimensional plane; Step S1.4: Considering the road scenario, the vehicle needs to approach the center line of the road while approaching the target point. Therefore, the objective function expression is: Where s = [s(0) T ,s(1) T ,…,s(N)] T Represents the state quantity at all times, u=[u(0) T ,u(1) T ,…,u(N-1)] T represents the control quantity at all times, ω g ,ω e ,ω c are the weights of approaching the end point, approaching the center line of the road, and reducing control consumption; x g is the state quantity of the end point, ||e(k)|| 2 =e p (k) 2 +ω θ e θ (k) 2 is the penalty for deviation from the center line of the road at time k, where e p (k) and e θ (k) represents the lateral error and heading angle error between the midpoint of the vehicle's rear axle and the centerline of the road, ω θ is the weight of the heading angle error; the road centerline is discretized into a set in is the set of midpoints of the road centerline, P cen [k] is the position of the kth point in the set, (x cen [k],y cen [k]) are the positions of the kth point in the set on the two-dimensional plane, θ cen [k] is the heading angle of the kth point in the set, is the number of points in the center line set; To derive e p (k) and e θ The analytical expression of (k) is as follows: Let the initial guess of the vehicle position in the optimization problem be Where (x init (k),y init (k)) is the initial guess of the vehicle position at time k; then, point (x init (k),y init (k)) Projected onto the road centerline, we get a set of reference points in, is (x init (k),y init (k)) The position of the projection point on the road centerline, and its corresponding relationship is: Assume that the center line of the road is composed of straight line segments and circular arc segments, and the projection point of the variable (x(k), y(k)) is ref (k),y ref (k)) are in the same straight line / arc segment. For the straight line segment, calculate e p (k) and e θ The expression of (k) is: e p (k)=(y(k)-y ref (k))cosθ ref (k)-(x(k)-x ref (k))sinθ ref (k) e θ (k)=θ(k)-θ ref (k) For circular segments, calculate and The expression is: e θ (k)=θ(k)-θ ref (k)+θ d (k) Among them, (x R (k),y R (k)) is the position of the center of the arc segment, R(k) is the radius of the arc segment corresponding to time k, θ d (k) is the position of the vehicle at time k (x(k), y(k)) and the position of the reference point (x ref (k),y ref (k)) corresponds to the arc center (x R (k),y R (k)) angle difference.

3. The rear-wheel steering vehicle trajectory planning method based on iterative optimization in road scenes according to claim 2 is characterized in that: The step S2 comprises: Step S2.1: Segment the centerline and identify local obstacles; Step S2.1.1: Classification of points on the centerline and identification of local obstacles; For each point on the centerline, it is divided into two categories: wide and narrow according to its collision risk with surrounding obstacles along the centerline; among them, for the kth point P on the centerline cen [k]=(x cen [k],y cen [k],θ cen [k]), if any vehicle position in a certain neighborhood With any obstacles Obs j ,j=1,...,N obs If no collision occurs, it is considered that there is no collision risk; the calculation standard is: Among them, Obs is the obstacle set {Obs j ,j=1,...,N obs }, CheckCollisionRisk(P cen ,Obs) is the judgment point P cen Function of the collision risk with obstacle Obs; For point P cen The surrounding area is expressed as: where p thre and θ thre are the thresholds for position and angle respectively; If there is no collision risk, then point P is considered cen [k]=(x cen [k],y cen [k],θ cen [k]) is classified as wide, otherwise it is classified as narrow; at the same time, according to the distance between the obstacle and point P cen [k], and identify P cen [k] nearby local obstacles, specifically: if the obstacle Obs j A vertex of cen If the distance between [k] is within the threshold Δs, then Obs j Point P cen [k] is in the surrounding obstacle set LocObs[k]; Step S2.1.2: centerline segmentation; Judgment point P cen After classification of [k], if it belongs to wide, it is classified into the wide point set, otherwise it is classified into the narrow point set; when the point changes from wide to narrow, the current wide point set is classified into the wide road section set, and the current wide point set is set to the empty set; conversely, when it changes from narrow to wide, the current narrow point set is classified into the narrow road section set, and the current narrow point set is set to the empty set; repeat this operation, traverse the points on the center line, and obtain multiple wide / narrow road sections; among which the i-th road section is represented as Among them, (x[k p ],y[k p ],θ[k p ]) is the kth p The indexed point, p is the index of the point in the set; represents the number of points in the i-th road section; Step S2.2: planning a rough path; A local coarse path is planned for each road section. For the i-th road section, if it is a wide road section, the road section itself is used as the coarse path. If it is a narrow road section, the coarse path is planned using the A* algorithm. Then, the road sections are connected and combined in sequence to obtain a global coarse path. Then, the global coarse path is spatially resampled to obtain a coarse path. in is the coarse path, N curr is the number of sampling points, x init [k],y init [k] is the position of the kth path point in the plane coordinate system; Step S2.3: Calculate the rough trajectory; Since the rough trajectory only contains position information, the rough trajectory is calculated based on the obtained rough trajectory What is missing from The expression is as follows: Among them, x init (k),y init (k),θ init (k), v init (k) are respectively x(k), y(k), θ(k), φ at time k f (k),φ r (k), the initial guess of v(k), v d (k) is the speed direction of the vehicle at time k, sgn is the sign function, Δt c =T / N curr The number of sampling points is N curr The sampling time below.

4. The rear-wheel steering vehicle trajectory planning method based on iterative optimization in road scenes according to claim 3 is characterized in that: The step S3 comprises: Step S3.1: design a penalty function to soften the kinematic constraints; For the system kinematic constraint Δs(k)=f(s(k),u(k)), the penalty function is designed as: Add the penalty function to the objective function to obtain a new objective function; Step S3.2: Design space-time corridor obstacle avoidance constraints; For the kth step in the prediction time domain, the predicted robot position is: (x init (k),y in it(k)); For the predicted robot position, initialize the linear obstacle avoidance constraints: x min (k)=x init (k)=x max (k) and min (k)=y init (k)=y max (k) After that, we can loop to get x as follows: min (k),x max (k),y min (k),y max (k): x min (k)=x min (k)-γ if C(x min (k)-γ:x max (k),y min (k):y max (k))=0 x max (k)=x max (k)+γ if C(x min (k):x max (k)+γ,y min (k):y max [k])=0 and min (k)=y min (k)-γ if C(x min (k):x max (k),y min (k)-γ:y max (k))=0 x max (k)=x max (k)+γ if C(x min (k):x max (k),y min (k):y max (k)+γ)=0 Among them, γ is the single step length, C(x min :x max ,y min :y max ) is to convert x min :x max ,y min :y max Function that projects space to a two-dimensional grid map. Its return value is 0 for x min :x max ,y min :y max There are no obstacles in the space, otherwise there are obstacles. The final space-time corridor constraints are as follows: x min (k)≤x(k)≤x max (k), y min (k)≤y(k)≤y max (k),#(6) k=0,…,N curr Indicates that the mobile robot's position is within the local rectangular frame; Step S3.3: solving the approximate feasible trajectory; By transforming the kinematic constraints into penalty terms in the objective function and constructing the space-time corridor constraints, the optimal control problem is constructed as follows: st kinematic constraint formula (2), Boundary condition constraint formula (3), The space-time corridor constraint formula (6) is: Among them, s,u are the variables to be solved, with T init is the initial guess, and a nonlinear solver is used to solve the approximate feasible trajectory.

5. The rear-wheel steering vehicle trajectory planning method based on iterative optimization in road scenarios according to claim 4 is characterized in that: The step S4 comprises: Step S4.1: construct and solve the optimal trajectory under the current sampling point; Based on the center line of the road, the obstacle avoidance function is reconstructed, and the initial guess Calculate its projection on the center line For the initial guess point P at time k init (k), if it is classified as wide and is at the projection point P ref (k) Surrounding, i.e. Then the obstacle avoidance constraint is simplified as: Where P(k) = (x(k), y(k), θ(k)) is the position of the vehicle at time k; otherwise, only the position with respect to the projection point P is considered. ref The obstacle avoidance constraint of the local obstacle Locobs(k) corresponding to (k) is: Where Δs is the radius of the local area; At this point, the optimization problem is constructed as: st kinematic constraints (1) (2), Boundary condition constraint formula (3), Simplify the obstacle avoidance constraint formula (7) and formula (8), Take the trajectory obtained in step S3.3 is the initial guess, and a nonlinear solver is used to solve the optimal trajectory at the current sampling point. Step S4.2: iteratively increase sampling points; The optimal trajectory of more sampling points is gradually solved by iteratively increasing the sampling points. In each iteration, the current sampling points are increased to α times the original number, and the optimal trajectory under the original number of sampling points is resampled to the trajectory of the current number of sampling points according to time. This is used as the initial solution. According to the method in step S4.1, the optimal trajectory under the current sampling point is calculated, and this cycle is repeated until the current number of sampling points N curr Reach the threshold sampling point N thre .

6. A rear-wheel steering vehicle trajectory planning system in a road scenario based on iterative optimization, characterized in that: include: Module M1: Establish a kinematic model for a rear-wheel steering vehicle. Based on the geometric relationship in the road scene, establish an analytical expression for the distance and angle error between the midpoint of the vehicle's rear axle and the centerline of the road, and formulate the trajectory planning problem as an optimization problem. Module M2: Based on the collision risk between the centerline and surrounding obstacles, the road centerline is divided into multiple wide and narrow local sections. At the same time, for each point in the centerline set, the corresponding local obstacle is found; a local coarse path is planned for each local section, and then combined into a complete global coarse path; Based on the global rough trajectory, the rough trajectory is derived as the initial guess for the subsequent optimization problem; Module M3: For the problem that the global rough trajectory is collision-free but kinematically infeasible, the kinematic constraints are softened into the objective function as penalty functions, and linear obstacle avoidance constraints are used to construct and solve a simplified optimization problem to obtain an approximate kinematically feasible trajectory; Module M4: Based on wide and narrow local road sections and local obstacles, obstacle avoidance constraints are constructed between the vehicle and the local obstacles. By gradually increasing the sampling points in the optimization problem, the optimization problem with a smaller number of sampling points is used to warm-start the subsequent optimization problem with a larger number of sampling points. The optimal trajectory is solved iteratively until the number of sampling points reaches the threshold, and finally the trajectory planning of the rear-wheel steering vehicle in the road scenario is realized.

7. The rear-wheel steering vehicle trajectory planning system based on iterative optimization in road scenes according to claim 6 is characterized in that: The module M1 comprises: Module M1.1: Kinematic modeling; In the fixed planning time domain T, the number of sampling points is N. The first-order Euler discretization method is used to set the rear axle midpoint position (x0, y0), the heading angle θ, and the wheel steering angle of the rear-wheel steering vehicle. Rear wheel steering angle With the rear wheel speed v as the state quantity, set the front wheel steering angle angular velocity ω f 、Rear wheel steering angle angular velocity ω r With the rear wheel acceleration a as the control variable, the vehicle motion formula under discrete time is obtained as follows: Where Δt is the sampling time Δt = T / N; s(k) = [x(k), y(k), θ(k), φ f (k),φ r (k),v(k)] T is the state quantity at the kth moment, u(k)=[ω f (k),ω r (k),a(k)] T is the control quantity at the kth moment, Δ is the difference of the state / control quantity at adjacent moments; f(s(k),u(k)) is the kinematic equation of the system; At the moment k = 0, ..., N, the front and rear wheel steering angles and angular velocities, rear wheel velocities and accelerations of the vehicle are subject to physical constraints, and the maximum / minimum constraints of the design state quantities and control quantities are as follows: in, v max They are the maximum front wheel turning angle, maximum rear wheel turning angle and maximum rear wheel linear speed of the vehicle, a max They are the maximum front wheel angular velocity, maximum rear wheel angular velocity and maximum rear wheel acceleration of the vehicle respectively; Module M1.2: The state of the vehicle at time k = 0 is the current state of the vehicle, and the design boundary condition constraints are as follows: s(0)=s start #(3) Module M1.3: In road scenes, use convex polygons to model other vehicles and other obstacles. Among them, Obs j is the jth obstacle, is the 1st,…,Nth j Vertices, N j is the number of vertices of the jth obstacle, N obs is the number of obstacles; the shape of the vehicle is approximated by a rectangle, and the vertices of the vehicle at time k are represented as Veh(k): = {A(k), B(k), C(k), D(k)}, where Veh(k) represents the vertex geometry of the vehicle at time k, and A(k), B(k), C(k), and D(k) represent the four vertices of the rectangle covering the vehicle body; at time k = 0, ..., N, the vehicle needs to avoid collision with obstacles, so the obstacle avoidance constraint is expressed as: OutPoly(a,{b1,…,b n }) function represents the point a in the convex polygon {b1,…,b n }, where n is the number of sides of the polygon; Among them, S (·) represents the area of ​​a triangle or convex polygon, ∈ is the relaxation parameter, where the area of ​​the convex polygon is calculated offline. The area calculation formula is: in, Points a and b i ,b i+1 Coordinate values ​​in the two-dimensional plane; Module M1.4: Considering the road scenario, the vehicle needs to approach the center line of the road while approaching the target point. Therefore, the objective function expression is: Where s = [s(0) T ,s(1) T ,…,s(N)] T Represents the state quantity at all times, u=[u(0) T ,u(1) T ,…,u(N-1)] T represents the control quantity at all times, ω g ,ω e ,ω c are the weights of approaching the end point, approaching the center line of the road, and reducing control consumption; x g is the state quantity of the end point, ||e(k)|| 2 =e p (k) 2 +ω θ e θ (k) 2 is the penalty for deviation from the center line of the road at time k, where e p (k) and e θ (k) represents the lateral error and heading angle error between the midpoint of the vehicle's rear axle and the centerline of the road, ω θ is the weight of the heading angle error; the road centerline is discretized into a set in is the set of midpoints of the road centerline, P cen [k] is the position of the kth point in the set, (x cen [k],y cen [k]) are the positions of the kth point in the set on the two-dimensional plane, θ cen [k] is the heading angle of the kth point in the set, is the number of points in the center line set; To derive e p (k) and e θ The analytical expression of (k) is as follows: Let the initial guess of the vehicle position in the optimization problem be Where (x init (k),y init (k)) is the initial guess of the vehicle position at time k; then, point (x init (k),y init (k)) Projected onto the road centerline, we get a set of reference points in, is (x init (k),y init (k)) The position of the projection point on the road centerline, and its corresponding relationship is: Assume that the center line of the road is composed of straight line segments and circular arc segments, and the projection point of the variable (x(k), y(k)) is ref (k),y ref (k)) are in the same straight line / arc segment. For the straight line segment, calculate e p (k) and e θ The expression of (k) is: e p (k)=(y(k)-y ref (k))cosθ ref (k)-(x(k)-x ref (k))sinθ ref (k) e θ (k)=θ(k)-θ ref (k) For circular segments, calculate and The expression is: e θ (k)=θ(k)-θ ref (k)+θ d (k) Among them, (x R (k),y R (k)) is the position of the center of the arc segment, R(k) is the radius of the arc segment corresponding to time k, θ d (k) is the position of the vehicle at time k (x(k), y(k)) and the position of the reference point (x ref (k),y ref (k)) corresponds to the arc center (x R (k),y R (k)) angle difference.

8. The rear-wheel steering vehicle trajectory planning system based on iterative optimization in road scenarios according to claim 7, characterized in that: The module M2 comprises: Module M2.1: Centerline segmentation and identification of local obstacles; Module M2.1.1: Classification of points on the centerline and identification of local obstacles; For each point on the centerline, it is divided into two categories: wide and narrow according to its collision risk with surrounding obstacles along the centerline; among them, for the kth point P on the centerline cen [k]=(x cen [k],y cen [k],θ cen [k]), if any vehicle position in a certain neighborhood With any obstacles Obs j ,j=1,...,N obs If no collision occurs, it is considered that there is no collision risk; the calculation standard is: Among them, Obs is the obstacle set {Obs j ,j=1,...,N obs }, CheckCollisionRisk(P cen ,Obs) is the judgment point P cen Function of the collision risk with obstacle Obs; For point P cen The surrounding area is expressed as: where p thre and θ thre are the thresholds for position and angle respectively; If there is no collision risk, then point P is considered cen [k]=(x cen [k],y cen [k],θ cen [k]) is classified as wide, otherwise it is classified as narrow; at the same time, according to the distance between the obstacle and point P cen [k], and identify P cen [k] nearby local obstacles, specifically: if the obstacle Obs j A vertex of cen If the distance between [k] is within the threshold Δs, then Obs j Point P cen [k] is in the surrounding obstacle set LocObs[k]; Module M2.1.2: Centerline segmentation; Judgment point P cen After classification of [k], if it belongs to wide, it is classified into the wide point set, otherwise it is classified into the narrow point set; when the point changes from wide to narrow, the current wide point set is classified into the wide road section set, and the current wide point set is set to the empty set; conversely, when it changes from narrow to wide, the current narrow point set is classified into the narrow road section set, and the current narrow point set is set to the empty set; repeat this operation, traverse the points on the center line, and obtain multiple wide / narrow road sections; among which the i-th road section is represented as Among them, (x[k p ],y[k p ],θ[k p ]) is the kth p The indexed point, p is the index of the point in the set; represents the number of points in the i-th road section; Module M2.2: Planning the rough path; A local coarse path is planned for each road section. For the i-th road section, if it is a wide road section, the road section itself is used as the coarse path. If it is a narrow road section, the coarse path is planned using the A* algorithm. Then, the road sections are connected and combined in sequence to obtain a global coarse path. Then, the global coarse path is spatially resampled to obtain a coarse path. in is the coarse path, N curr is the number of sampling points, x init [k],y init [k] is the position of the kth path point in the plane coordinate system; Module M2.3: Calculate rough trajectory; Since the rough trajectory only contains position information, the rough trajectory is calculated based on the obtained rough trajectory What is missing from The expression is as follows: θ init (k+1)=tan -1 (Δy init (k),Δx init (k)),k=0,…,N curr -1 v d (k)=Δx init (k)cosθ init (k)+Δy init (k)sinθ init (k) v init (k)=sgn(v d (k))·||(Δx init (k),Δy init (k))|| / Δt c v init (N curr )=v init (N curr -1) Among them, x init (k),y init (k),θ init (k), v init (k) are respectively x(k), y(k), θ(k), φ at time k f (k),φ r (k), the initial guess of v(k), v d (k) is the speed direction of the vehicle at time k, sgn is the sign function, Δt c =T / N curr The number of sampling points is N curr The sampling time below.

9. The rear-wheel steering vehicle trajectory planning system based on iterative optimization in road scenarios according to claim 8, characterized in that: The module M3 comprises: Module M3.1: Design penalty functions to soften kinematic constraints; For the system kinematic constraint Δs(k)=f(s(k),u(k)), the penalty function is designed as: Add the penalty function to the objective function to obtain a new objective function; Module M3.2: Design space-time corridor obstacle avoidance constraints; For the kth step in the prediction time domain, the predicted robot position is: (x init (k),y init (k)); For the predicted robot position, initialize the linear obstacle avoidance constraints: x min (k)=x init (k)=x max (k) and min (k)=y init (k)=y max (k) After that, we can loop to get x as follows: min (k),x max (k),y min (k),y max (k): x min (k)=x min (k)-γ if C(x min (k)-γ:x max (k),y min (k):y max (k))=0 x max (k)=x max (k)+γ if C(x min (k):x max (k)+γ,y min (k):y max [k])=0 and min (k)=y min (k)-γ if C(x min (k):x max (k),y min (k)-γ:y max (k))=0 x max (k)=x max (k)+γ if C(x min (k):x max (k),y min (k):y max (k)+γ)=0 Among them, γ is the single step length, C(x min :x max ,y min :y max ) is to convert x min :x max ,y min :y max Function that projects space to a two-dimensional grid map. Its return value is 0 for x min :x max ,y min :y max There are no obstacles in the space, otherwise there are obstacles. The final space-time corridor constraints are as follows: Indicates that the mobile robot's position is within the local rectangular frame; Module M3.3: Solving approximate feasible trajectories; By transforming the kinematic constraints into penalty terms in the objective function and constructing the space-time corridor constraints, the optimal control problem is constructed as follows: st kinematic constraint formula (2), Boundary condition constraint formula (3), The space-time corridor constraint formula (6) is: Among them, s and u are the variables to be solved. is the initial guess, and a nonlinear solver is used to solve the approximate feasible trajectory.

10. The rear-wheel steering vehicle trajectory planning system based on iterative optimization in road scenes according to claim 9, characterized in that: The module M4 comprises: Module M4.1: Construct and solve the optimal trajectory under the current sampling point; Based on the center line of the road, the obstacle avoidance function is reconstructed, and the initial guess Calculate its projection on the center line For the initial guess point P at time k init (k), if it is classified as wide and is at the projection point P ref (k) Surrounding, i.e. Then the obstacle avoidance constraint is simplified as: Where P(k) = (x(k), y(k), θ(k)) is the position of the vehicle at time k; otherwise, only the position with respect to the projection point P is considered. ref The obstacle avoidance constraint of the local obstacle Locobs(k) corresponding to (k) is: Where Δs is the radius of the local area; At this point, the optimization problem is constructed as: st kinematic constraints (1) (2), Boundary condition constraint formula (3), Simplify the obstacle avoidance constraint formula (7) and formula (8), The trajectory obtained in module M3.3 is the initial guess, and a nonlinear solver is used to solve the optimal trajectory at the current sampling point. Module M4.2: Iteratively increase sampling points; The optimal trajectory of more sampling points is gradually solved by iteratively increasing the sampling points. In each iteration, the current sampling points are increased to α times the original number, and the optimal trajectory under the original number of sampling points is resampled to the trajectory of the current number of sampling points according to time. This is used as the initial solution. According to the method in module M4.1, the optimal trajectory under the current sampling point is calculated, and this cycle is repeated until the current number of sampling points N curr Reach the threshold sampling point N thre .

Images