High-precision trajectory planning method for intelligent vehicle in irregular form

Abstract

The invention relates to the technical field of intelligent vehicle trajectory planning, and particularly discloses a high-precision trajectory planning method for an irregular-form intelligent vehicle, which is technically characterized by comprising two steps: step 1, modeling a trajectory planning problem into a constrained dynamic optimization problem, in the modeling process, the contour of the irregular special-shaped vehicle is accurately expressed, and in the second step, the dynamic optimization problem is solved through numerical values. The obstacle avoidance constraint in the method reflects a new modeling processing method for the special-shaped contour, and high-precision trajectory planning can be realized in an extremely narrow scene. The method is applied to a passenger car autonomous parking software system, the successful parking probability of a narrow parking space can be increased to 95% from original 70%, and the effect is remarkable.

Description

Technical Field

[0001] The invention patent relates to the technical field of intelligent vehicle trajectory planning, specifically, to a high-precision trajectory planning method for irregular-shaped intelligent vehicles. Background Art

[0002] The trajectory planning of an intelligent vehicle aims to generate a space-time trajectory that reflects the local path and speed of the vehicle, conforming to the vehicle's internal kinematic constraints and external environment obstacle avoidance constraints, and optimizing the established performance indicators as much as possible while strictly ensuring that the above constraints are met.

[0003] Trajectory planning is implemented on a two-dimensional plane. The shape of the smart car on the plane is generally considered to be a rectangle, which means that the collision avoidance constraint should be established as an expression that does not overlap between the rectangle and the surrounding obstacles. Figure 1 As shown in the figure, in extremely narrow scenarios, the outer contour of the vehicle is irregular. If it is described as a larger rectangular box, it will cause an overly conservative estimation of the vehicle body shape, making it difficult to implement trajectory planning smoothly.

[0004] At present, the main practice in the industry is to use multiple disks to cover the area to represent the vehicle body, or to use the external rectangular frame mentioned above to just cover the vehicle body. However, the obstacle avoidance constraints expressed by the above methods will introduce more redundant space (such as Figure 2 shown).

[0005] In short, the current research on accurate trajectory planning technology for vehicles with irregular outer contours on the market is not sufficient.

[0006] To this end, the inventors of the present application propose a high-precision trajectory planning method for an irregular-shaped intelligent vehicle to solve the above-mentioned problems in the prior art. Summary of the invention

[0007] The purpose of the present invention is to solve the problems of the prior art mentioned in the above background technology and to provide a high-precision trajectory planning method for an irregular-shaped intelligent vehicle.

[0008] The above-mentioned purpose of the present invention is achieved like this:

[0009] A high-precision trajectory planning method for an irregular-shaped intelligent vehicle includes the following steps:

[0010] S1. Trajectory planning problem modeling:

[0011] Accurately construct the trajectory planning problem considering the irregular profile of the vehicle, which is a constrained dynamic optimization problem consisting of decision variables, constraints and cost functions;

[0012] S2. Solution of trajectory planning problem:

[0013] After constructing the model of the trajectory planning problem, it is solved and the dynamic optimization problem is solved using numerical optimization methods.

[0014] Further, in step S1, specifically:

[0015] S1-1. Decision variables:

[0016] 1) Define (x(t), y(t)) to represent the coordinates of a fixed reference point on the vehicle body in the Earth's inertial coordinate system at time t;

[0017] 2) Define v(t) and a(t) to represent the velocity and acceleration of the vehicle body reference point, respectively;

[0018] 3) Define φ(t) as the front wheel deflection angle of the vehicle, ω(t) as the angular velocity of the front wheel deflection angle, and θ(t) as the attitude angle of the vehicle in the geodetic inertial coordinate system;

[0019] Then the decision variables include the above x(t), y(t), v(t), a(t), φ(t), ω(t) and θ(t);

[0020] S1-2. Cost function:

[0021] The cost function in the dynamic optimization problem model is used to reflect the stability and comfort of vehicle driving. Assuming that the time domain of trajectory planning is t∈[0,T], the cost function J is constructed as:

[0022]

[0023] Among them, w a ,w w >0 is the weight specified by the user;

[0024] S1-3. Constraints:

[0025] The constraints in the dynamic optimization problem model include vehicle kinematic constraints, two-point boundary value constraints, and obstacle avoidance constraints, specifically:

[0026] a) Vehicle kinematic constraints:

[0027] The motion of the vehicle in the inertial coordinate system is subject to the following differential equations:

[0028]

[0029] Where L W Represents the wheelbase of the vehicle;

[0030] At the same time, some decision variables have an allowable action range, and corresponding interval constraints are imposed:

[0031]

[0032] Among them, a min with a max Represents the left and right boundary values ​​of acceleration, v min With v max Represents the left and right boundary values ​​of the velocity, Ω max represents the maximum amplitude of the front wheel angular velocity ω(t), Φ max Represents the maximum amplitude of the front wheel steering angle φ(t);

[0033] b) Two-point boundary value constraints:

[0034] At the starting time t=0 of the trajectory planning time domain, the vehicle is in the current actual motion state. At this time, the acceleration is a0, the speed is v0, the attitude angle is theta0, the front wheel angle is phi0, the front wheel angular velocity is omega0, the horizontal coordinate position is x0, and the vertical coordinate position is y0. The following constraints are imposed:

[0035]

[0036] At the end time t = T in the trajectory planning time domain, the vehicle is expected to be in a specified motion state. If the acceleration is af, the velocity is vf, the attitude angle is thetaf, the front wheel angle is phif, the front wheel angular velocity is omegaf, the horizontal coordinate position is xf, and the vertical coordinate position is yf, then the following constraints are imposed:

[0037]

[0038] c) Obstacle avoidance constraints:

[0039] At every moment in the trajectory planning domain t∈[0,T], the vehicle should effectively avoid collisions with obstacles in the environment; specifically:

[0040] (1) First, obtain the irregular outer contour trend diagram of the vehicle and find the geometric center point of the contour diagram;

[0041] (2) Line segments are emitted in a 360-degree range around the central point image, and each line segment is ensured to end on the outer contour of the special-shaped vehicle, and the length of each emitted line segment is determined accordingly;

[0042] (3) With the longitudinal axis of the vehicle and the forward direction as the reference 0 degree, define the deflection angle, and the deflection angle ranges from 0 to 360 degrees; based on the actual position of the outer contour, draw a trend line graph of the length of the line segment within the 360-degree range versus the deflection angle;

[0043] (4) Using quintic polynomial interpolation, a finite number of angle values ​​are interpolated to fit a function of the length of a line segment defined everywhere between 0 and 360 degrees as the deflection angle changes. The deflection angle is abbreviated as ang, and the function of the length of the line segment as the deflection angle changes is simply recorded as Func(ang), where ang∈[0,360°];

[0044] (5) All static obstacles in the environment are represented as scattered points, and a point set S is defined to store all the scattered points of the obstacles:

[0045] S={(wx i ,wy i )i=1,...,N};

[0046] Each element in the set S records the horizontal and vertical coordinate values ​​of the scattered points, and the total number of scattered points is agreed to be N;

[0047] (6) Describe the coordinate position of the vehicle's geometric center point Q at the current moment: The center point of the vehicle's rear wheels is currently at (x(t), y(t)), and the geometric center point Q = (xq(t), yq(t)) must also fall on the vehicle's longitudinal axis. Assuming that the geometric center point is located at a distance L in front of the center point of the rear wheels, the coordinate value of the geometric center point is defined as:

[0048]

[0049] (7) For a certain obstacle scatter point (wx i ,wy i ), connect it with the vehicle center point Q = (xq(t), yq(t)), and the angle of the connecting line and the reference 0 degree direction angle β along the longitudinal axis of the vehicle body are expressed as:

[0050]

[0051] Obstacle scatter points (wx i ,wy i ) does not collide with the vehicle body, then the distance QW is greater than the distance QM:

[0052] That is, the constraint is expressed as:

[0053]

[0054] Further refine it into:

[0055]

[0056] At this point, the obstacle avoidance constraint considering the contour of the special-shaped vehicle is constructed as expression (6);

[0057] The complete trajectory planning problem is then formulated as the following dynamic optimization problem:

[0058]

[0059] Furthermore, step S2 solves the model formula (7) of the successfully constructed trajectory planning problem, and adopts a numerical optimization method to solve the dynamic optimization problem, wherein the numerical optimization method is an interior point algorithm or a sequential quadratic programming algorithm.

[0060] Furthermore, the specific method of the interior point algorithm to solve the dynamic optimization problem is:

[0061] First, the dynamic optimization problem to be solved can be concisely described as the following abstract problem:

[0062]

[0063] Where χ represents the decision variable. By introducing the relaxation vector s>0, the inequality constraint g(χ)<0 in (A1) can be transformed into an equality constraint, that is:

[0064]

[0065] At this point, if the only inequality constraint s>0 in Proposition (A2) is converted into an internal penalty function term and added to the cost function J, a standard form constrained optimization problem containing only equality constraints can be constructed:

[0066]

[0067] Among them, J μ (χ,s) represents the cost function including the internal penalty function term; μ IPM >0 is the barrier factor, and its value is closer to 0 + Then through the internal penalty function μ IPM ln(s) describes inequalities s>0 more accurately.

[0068] Furthermore, the interior point algorithm is implemented as follows:

[0069] start;

[0070] Abstract the actual trajectory planning problem into an optimization problem (A1);

[0071] Initialize μ IPM ,threshold,

[0072] while(μ IPM >threshold),do

[0073] Update μ IPM ←μ IPM 0.5;

[0074] Under the current internal penalty function weight, (A1) is expressed as (A3);

[0075] The equality constraints in (A3) are inserted into the cost function of (A3) in the form of Lagrange multiplier polynomials, thereby converting (A3) into an unconstrained optimization problem (temporarily denoted as prob);

[0076] Solve the unconstrained optimization problem prob using Newton's method;

[0077] Derive Newton's method to solve the decision variable χ of prob;

[0078] endwhile

[0079] Output χ;

[0080] Finish.

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

[0082] 1. The obstacle avoidance constraints in the method of the present invention reflect a new modeling and processing method for special-shaped contours, which can achieve high-precision trajectory planning in extremely narrow scenes;

[0083] 2. The method of the present invention is applied to the autonomous parking software system of passenger cars, which can increase the probability of successful parking in narrow parking spaces from the original 70% to 95%, with significant effects. BRIEF DESCRIPTION OF THE DRAWINGS

[0084] Figure 1 It is a schematic diagram of the irregularity of the vehicle's outer contour in an extremely narrow scene;

[0085] Figure 2 It is a schematic diagram of a method for expressing obstacle avoidance constraints in the prior art;

[0086] Figure 3 is a trend diagram of an irregular outer contour of a vehicle in an embodiment of the present invention;

[0087] Figure 4 is a schematic diagram of emitting line segments within a 360-degree range around a central point image in an embodiment of the present invention;

[0088] Figure 5 is a schematic diagram of the definition of the deflection angle in an embodiment of the present invention;

[0089] Figure 6 is a trend line graph of the length of a line segment within a range of 360 degrees versus the deflection angle in an embodiment of the present invention;

[0090] Figure 7 2 is a schematic diagram showing that the distance QW is greater than the distance QM in an embodiment of the present invention. DETAILED DESCRIPTION

[0091] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0092] The implementation of the present invention is described in detail below in conjunction with specific embodiments. For those skilled in the art, the specific meanings of the above terms can be understood according to specific circumstances.

[0093] The following are preferred embodiments of the present invention.

[0094] The embodiment of the present invention provides a high-precision trajectory planning method for an irregular-shaped smart car. The trajectory planning method is divided into two steps. The first step is to model the trajectory planning problem as a constrained dynamic optimization problem, and to accurately express the contour of the irregular-shaped vehicle in the modeling process. The second step is to numerically solve the dynamic optimization problem. Specifically:

[0095] Step 1: Modeling the trajectory planning problem

[0096] Constructing an accurate model for the trajectory planning problem is an important basis for implementing a smooth solution. This section introduces how to accurately construct a trajectory planning problem that takes into account the irregular and special-shaped contours of the vehicle.

[0097] Like the modeling of all trajectory planning problems, the present invention constructs it as a constrained dynamic optimization problem. This optimization problem consists of decision variables, constraints and cost functions. The modeling process is explained in detail below. The core point that reflects the creativity of the method of the present invention lies in the obstacle avoidance constraint, because the obstacle avoidance constraint reflects how the method of the present invention models the special-shaped contour.

[0098] Decision variables:

[0099] The decision variables in the trajectory planning problem model based on dynamic optimization include the following:

[0100] 1)(x(t),y(t)), represents the coordinates of a fixed reference point of the vehicle body in the earth's inertial coordinate system at time t;

[0101] 2) v(t) and a(t) represent the velocity and acceleration of the vehicle body reference point, respectively;

[0102] 3)φ(t) is the front wheel deflection angle of the vehicle, ω(t) is the angular velocity corresponding to the front wheel deflection angle, and θ(t) represents the attitude angle of the vehicle in the earth's inertial coordinate system.

[0103] Therefore, the decision variables include the above-mentioned x(t), y(t), v(t), a(t), φ(t), ω(t) and θ(t).

[0104] Cost function:

[0105] The cost function in the dynamic optimization problem model is generally used to reflect the stability and comfort of vehicle driving. Assuming that the time domain of trajectory planning is t∈[0,T], the cost function J is constructed as:

[0106]

[0107] Among them, w a ,w w >0 is the weight specified by the user.

[0108] constraint:

[0109] The constraints in the dynamic optimization problem model include vehicle kinematic constraints, two-point boundary value constraints and obstacle avoidance constraints, among which the vehicle kinematic constraints and the two-point boundary value constraints adopt existing technologies, and the modeling method of the obstacle avoidance constraint is the core point of the method of the present invention.

[0110] a) Vehicle kinematic constraints:

[0111] The motion of the vehicle in the inertial coordinate system is subject to the following differential equations:

[0112]

[0113] Where L W Represents the vehicle's wheelbase.

[0114] At the same time, some decision variables have an allowable action range, so corresponding interval constraints should be imposed:

[0115]

[0116] Among them, a min with a max Represents the left and right boundary values ​​of acceleration, v min With v max Represents the left and right boundary values ​​of the velocity, Ω max represents the maximum amplitude of the front wheel angular velocity ω(t), Φ max Represents the maximum amplitude of the front wheel steering angle φ(t).

[0117] b) Two-point boundary value constraints:

[0118] At the starting time t=0 in the trajectory planning domain, the vehicle is in the current actual motion state. For example, at this time, the acceleration is a0, the speed is v0, the attitude angle is theta0, the front wheel angle is phi0, the front wheel angular velocity is omega0, the horizontal coordinate position is x0, and the vertical coordinate position is y0. Then the following constraints should be imposed:

[0119]

[0120] Similarly, at the end time t = T in the trajectory planning domain, the vehicle is expected to be in a specified motion state, for example, the acceleration is af, the velocity is vf, the attitude angle is thetaf, the front wheel angle is phif, the front wheel angular velocity is omegaf, the horizontal coordinate position is xf, and the vertical coordinate position is yf, then the following constraints should be imposed:

[0121]

[0122] c) Obstacle avoidance constraints:

[0123] At every moment in the trajectory planning domain t∈[0,T], the vehicle should effectively avoid collision with obstacles in the environment.

[0124] First, get the irregular outer contour trend map of the vehicle and find the geometric center point of the contour map (such as Figure 3 shown).

[0125] Then, line segments are emitted in a 360-degree range around the center point image, and each line segment is ensured to end on the outer contour of the special-shaped vehicle. The length of each emitted line segment is determined accordingly (e.g. Figure 4 shown):

[0126] Next, with the vehicle's longitudinal axis and the forward direction as the reference 0 degrees, the deflection angle is defined as Figure 5 As shown in the figure, the deflection angle ranges from 0 to 360°. Combined with the actual position of the outer contour, a line graph showing the trend of the line segment length within the 360-degree range versus the deflection angle can be drawn (e.g. Figure 6 shown).

[0127] The figure below shows a schematic diagram of the trend line chart. The horizontal axis is the deflection angle, the domain is 0 to 360 degrees, and the vertical axis is the length of the line segment in meters:

[0128] Due to the density of real numbers, there are infinite angles between 0 and 360 degrees, so we can only select a finite number of angles between 0 and 360 and calculate the length of the line segment. There are always new real numbers between any two adjacent angles (density axiom of real number field in advanced mathematics). To solve this problem, the present invention performs the following steps.

[0129] Next, a finite number of angle values ​​are interpolated using quintic polynomial interpolation to fit a function of the length of a defined line segment everywhere between 0 and 360 degrees as the deflection angle changes. The deflection angle is now abbreviated as ang, and the function of the length of the line segment as the deflection angle changes is simply recorded as Func(ang), where ang∈[0,360°].

[0130] Next, all static obstacles in the environment are represented as scattered points. Let’s define a point set S to store all the scattered points of obstacles:

[0131] S={(wx i ,wy i )i=1,...,N}

[0132] Each element in the set S records the horizontal and vertical coordinate values ​​of the scattered point, and the total number of scattered points is agreed to be N.

[0133] Next, describe the coordinate position of the vehicle's geometric center point Q at the current moment. Specifically, the current vehicle rear wheel center point is at (x(t), y(t)), and the geometric center point Q = (xq(t), yq(t)) must also fall on the vehicle's longitudinal axis. Assume that the geometric center point is located L distance in front of the rear wheel center point, then the coordinate value of the geometric center point can be defined as

[0134] xq(t)=x(t)+L·cosθ(t)

[0135] yq(t)=y(t)+L·sinθ(t)

[0136] Next, for a certain obstacle scatter point (wx i ,wy i ), connect it with the vehicle center point Q = (xq(t), yq(t)), and the angle of the connecting line and the reference 0 degree angle β along the longitudinal axis of the vehicle body mentioned above are expressed as

[0137]

[0138] Obstacle scatter points (wx i ,wy i ) does not collide with the vehicle body, which means that the distance QW shown in the figure below is greater than the distance QM (such as Figure 7 shown).

[0139] Such a constraint is expressed as:

[0140]

[0141] It can be further refined into:

[0142]

[0143] At this point, the obstacle avoidance constraint considering the contour of the special-shaped vehicle is constructed as expression (6).

[0144] The complete trajectory planning problem is formulated as the following dynamic optimization problem:

[0145]

[0146] Step 2: Solving the trajectory planning problem

[0147] After successfully constructing the model of the trajectory planning problem (i.e., formula (7)), it should be solved. In the implementation of the present invention, a numerical optimization method is used to solve the dynamic optimization problem, such as an interior point algorithm or a sequential quadratic programming algorithm. For the interior point algorithm, see the following:

[0148] A. and L.Biegler, "On the implementation of an interior-pointfilter line-search algorithm for large-scale nonlinear programming," Mathematical Programming, vol.106, no.1, pp.25–57, 2006.

[0149] The following describes how the interior point algorithm solves the dynamic optimization problem (7).

[0150] First, the dynamic optimization problem to be solved can be concisely described as the following abstract problem:

[0151]

[0152] Where χ represents the decision variable. By introducing the relaxation vector s>0, the inequality constraint g(χ)<0 in (A1) can be transformed into an equality constraint, that is,

[0153]

[0154] At this point, if the only inequality constraint s>0 in Proposition (A2) is converted into an internal penalty function term and added to the cost function J, a standard form constrained optimization problem containing only equality constraints can be constructed:

[0155]

[0156] Among them, J μ (χ,s) represents the cost function including the internal penalty function term; μ IPM >0 is the barrier factor, and its value is closer to 0 + Then through the internal penalty function μIPM ln(s) describes inequalities s>0 more accurately.

[0157] The interior point algorithm solves Proposition (A1) by sequentially solving μ IPM The value gradually approaches 0 + The corresponding series of sub-problems are as follows (A3).

[0158] With μ IPM Approaching 0 + , the extreme value of the subproblem will tend to the extreme value of the original proposition (A1). In the process of sequential calculation, for a specific obstacle factor μ IPM The method for solving subproblem (A3) is Lagrange augmented Newton method. From the above analysis, we can see that the interior point algorithm solves NLP propositions through a two-layer loop architecture: in the outer loop, μ is updated. IPM , iteratively solve the given μ in the inner layer IPM Sub-problem (A3) under the condition.

[0159] Therefore, the execution of the interior point algorithm is as follows, and the pseudo code is given below:

[0160] start

[0161] Abstract the actual trajectory planning problem into an optimization problem (A1);

[0162] Initialize μ IPM ,threshold,

[0163] while(μ IPM >threshold),do

[0164] Update μ IPM ←μ IPM 0.5;

[0165] Under the current internal penalty function weight, (A1) is expressed as (A3);

[0166] The equality constraints in (A3) are inserted into the cost function of (A3) in the form of Lagrange multiplier polynomials, thereby converting (A3) into an unconstrained optimization problem (temporarily denoted as prob);

[0167] Solve the unconstrained optimization problem prob using Newton's method;

[0168] Derive Newton's method to solve the decision variable χ of prob;

[0169] end while

[0170] Output χ;

[0171] Finish.

[0172] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A high-precision trajectory planning method for an irregular-shaped intelligent vehicle, characterized in that: The following steps are involved: S1. Trajectory planning problem modeling: Accurately construct the trajectory planning problem considering the irregular profile of the vehicle, which is a constrained dynamic optimization problem consisting of decision variables, constraints and cost functions; S2. Solution of trajectory planning problem: After constructing the model of the trajectory planning problem, it is solved and the dynamic optimization problem is solved using numerical optimization methods.

2. The high-precision trajectory planning method for an irregular-shaped intelligent vehicle according to claim 1 is characterized in that: In step S1, specifically: S1-1. Decision variables: 1) Define (x(t), y(t)) to represent the coordinates of a fixed reference point on the vehicle body in the Earth's inertial coordinate system at time t; 2) Define v(t) and a(t) to represent the velocity and acceleration of the vehicle body reference point, respectively; 3) Define φ(t) as the front wheel deflection angle of the vehicle, ω(t) as the angular velocity of the front wheel deflection angle, and θ(t) as the attitude angle of the vehicle in the geodetic inertial coordinate system; Then the decision variables include the above x(t), y(t), v(t), a(t), φ(t), ω(t) and θ(t); S1-2. Cost function: The cost function in the dynamic optimization problem model is used to reflect the stability and comfort of vehicle driving. Assuming that the time domain of trajectory planning is t∈[0,T], the cost function J is constructed as: Among them, w a ,w w >0 is the weight specified by the user; S1-3. Constraints: The constraints in the dynamic optimization problem model include vehicle kinematic constraints, two-point boundary value constraints, and obstacle avoidance constraints, specifically: a) Vehicle kinematic constraints: The motion of the vehicle in the inertial coordinate system is subject to the following differential equations: Where L W Represents the wheelbase of the vehicle; At the same time, some decision variables have an allowable action range, and corresponding interval constraints are imposed: Among them, a min with a max Represents the left and right boundary values ​​of acceleration, v min With v max Represents the left and right boundary values ​​of the velocity, Ω max represents the maximum amplitude of the front wheel angular velocity ω(t), Φ max Represents the maximum amplitude of the front wheel steering angle φ(t); b) Two-point boundary value constraints: At the starting time t=0 of the trajectory planning time domain, the vehicle is in the current actual motion state. At this time, the acceleration is a0, the speed is v0, the attitude angle is theta0, the front wheel angle is phi0, the front wheel angular velocity is omega0, the horizontal coordinate position is x0, and the vertical coordinate position is y0. The following constraints are imposed: At the end time t = T in the trajectory planning time domain, the vehicle is expected to be in a specified motion state. If the acceleration is af, the velocity is vf, the attitude angle is thetaf, the front wheel angle is phif, the front wheel angular velocity is omegaf, the horizontal coordinate position is xf, and the vertical coordinate position is yf, then the following constraints are imposed: c) Obstacle avoidance constraints: At every moment in the trajectory planning domain t∈[0,T], the vehicle should effectively avoid collisions with obstacles in the environment; specifically: (1) First, obtain the irregular outer contour trend diagram of the vehicle and find the geometric center point of the contour diagram; (2) Line segments are emitted in a 360-degree range around the central point image, and each line segment is ensured to end on the outer contour of the special-shaped vehicle, and the length of each emitted line segment is determined accordingly; (3) With the longitudinal axis of the vehicle and the forward direction as the reference 0 degree, define the deflection angle, and the deflection angle ranges from 0 to 360 degrees; based on the actual position of the outer contour, draw a trend line graph of the length of the line segment within the 360-degree range versus the deflection angle; (4) Using quintic polynomial interpolation, a finite number of angle values ​​are interpolated to fit a function of the length of a line segment defined everywhere between 0 and 360 degrees as the deflection angle changes. The deflection angle is abbreviated as ang, and the function of the length of the line segment as the deflection angle changes is simply recorded as Func(ang), where ang∈[0,360°]; (5) All static obstacles in the environment are represented as scattered points, and a point set S is defined to store all the scattered points of the obstacles: S={(wx i ,wy i )i=1,...,N}; Each element in the set S records the horizontal and vertical coordinate values ​​of the scattered points, and the total number of scattered points is agreed to be N; (6) Describe the coordinate position of the vehicle's geometric center point Q at the current moment: The center point of the vehicle's rear wheels is currently at (x(t), y(t)), and the geometric center point Q = (xq(t), yq(t)) must also fall on the vehicle's longitudinal axis. Assuming that the geometric center point is located at a distance L in front of the center point of the rear wheels, the coordinate value of the geometric center point is defined as: (7) For a certain obstacle scatter point (wx i ,wy i ), connect it with the vehicle center point Q = (xq(t), yq(t)), and the angle of the connecting line and the reference 0 degree direction angle β along the longitudinal axis of the vehicle body are expressed as: Obstacle scatter points (wx i ,wy i ) does not collide with the vehicle body, then the distance QW is greater than the distance QM: That is, the constraint is expressed as: Further refine it into: At this point, the obstacle avoidance constraint considering the contour of the special-shaped vehicle is constructed as expression (6); The complete trajectory planning problem is then formulated as the following dynamic optimization problem:

3. The high-precision trajectory planning method for an irregular-shaped intelligent vehicle according to claim 2, characterized in that: Step S2 solves the model formula (7) of the successfully constructed trajectory planning problem, and adopts a numerical optimization method to solve the dynamic optimization problem. The numerical optimization method is an interior point algorithm or a sequential quadratic programming algorithm.

4. The high-precision trajectory planning method for an irregular-shaped intelligent vehicle according to claim 3 is characterized in that: The specific method of the interior point algorithm to solve the dynamic optimization problem is: First, the dynamic optimization problem to be solved can be concisely described as the following abstract problem: Where χ represents the decision variable. By introducing the relaxation vector s>0, the inequality constraint g(χ)<0 in (A1) can be transformed into an equality constraint, that is: At this point, if the only inequality constraint s>0 in Proposition (A2) is converted into an internal penalty function term and added to the cost function J, a standard form constrained optimization problem containing only equality constraints can be constructed: Among them, J μ (χ,s) represents the cost function including the internal penalty function term; μ IPM >0 is the barrier factor, and its value is closer to 0 + Then through the internal penalty function μ IPM ln(s) describes inequalities s>0 more accurately.

5. The high-precision trajectory planning method for an irregular-shaped intelligent vehicle according to claim 4 is characterized in that: The interior point algorithm is implemented as follows: start; Abstract the actual trajectory planning problem into an optimization problem (A1); Initialize μ IPM ,threshold, while(μ IPM >threshold),do Update μ IPM ←μ IPM 0.5; Under the current internal penalty function weight, (A1) is expressed as (A3); The equality constraints in (A3) are inserted into the cost function of (A3) in the form of Lagrange multiplier polynomials, thereby transforming (A3) into an unconstrained optimization problem, temporarily denoted as prob; Solve the unconstrained optimization problem prob using Newton's method; Derive Newton's method to solve the decision variable χ of prob; endwhile Output χ; Finish.

Images