Unmanned vehicle trajectory planning method, device, equipment and medium

Abstract

The invention discloses an unmanned vehicle trajectory planning method and device, equipment and a medium, and relates to the field of unmanned vehicle trajectory planning, and the method comprises the steps: carrying out the dynamic planning according to reference line information, obtaining the initial conjecture of an unmanned vehicle driving trajectory, carrying out the region division of a target road through the initial conjecture and the road shape of a target reference line, and obtaining the unmanned vehicle trajectory. Constructing a multi-reference-line penalty function according to the region division result, constructing an optimization-based trajectory smoothing model based on the multi-reference-line penalty function, and solving the optimization-based trajectory smoothing model by taking the initial conjecture as an initial iteration value to obtain a final trajectory of the unmanned vehicle. The method can converge the transverse error of the trajectory and the reference line composed of the straight line and the curve in the Cartesian coordinate system, does not depend on a traditional curve coordinate system, improves the quality of the trajectory, and reduces the risk of unmanned driving.

Description

Technical Field

[0001] The present application relates to the field of unmanned vehicle trajectory planning, and in particular to an unmanned vehicle trajectory planning method, device, equipment and medium. Background Art

[0002] Unmanned vehicles (abbreviated as unmanned vehicles) have become a research hotspot in recent years. As one of its key technologies, the local path planning of unmanned vehicles directly determines the success of the unmanned vehicle's driving.

[0003] The driving trajectory of the unmanned vehicle needs to meet the three requirements of smoothness, optimality and real-time performance. In addition, reference line constraints need to be considered in structured scenarios, that is, the vehicle's driving trajectory should be as close to the reference line as possible while ensuring safety. These reference lines are often the center lines of the road, and are also an important basis for judging the legality of vehicle behavior. Most of the popular methods currently use a curved coordinate system based on the reference line to complete trajectory planning that takes into account reference line constraints. However, there are several recognized shortcomings in the curved coordinate system: 1) It is impossible to simulate the true kinematics of the ego vehicle, making the trajectory prone to violating curvature restrictions, 2) Ignoring vehicle shape distortion, and 3) It is impossible to ensure trajectory continuity. Trajectory planning in a Cartesian system can overcome the shortcomings of a curved coordinate system, but there has been no mature solution for reference line constraints. Based on this, the present application provides a trajectory planning method for an autonomous driving vehicle that takes into account multiple reference line constraints in a variety of road line scenarios based on a Cartesian coordinate system. Summary of the invention

[0004] The purpose of this application is to provide a method, device, equipment and medium for unmanned vehicle trajectory planning, which can converge the lateral error of the trajectory and a reference line composed of straight lines and curves in a Cartesian coordinate system, without relying on a traditional curvilinear coordinate system, thereby improving the quality of the trajectory and reducing the danger of unmanned driving.

[0005] To achieve the above objectives, this application provides the following solutions.

[0006] In a first aspect, the present application provides a method for unmanned vehicle trajectory planning, comprising the following steps.

[0007] Obtain reference line information of a target road of the unmanned vehicle; the reference line information includes main reference line information and secondary reference line information; the secondary reference line information is determined by a lateral offset between the secondary reference line and the main reference line.

[0008] Dynamic planning is performed based on the reference line information to obtain an initial guess of the driving trajectory of the unmanned vehicle; the initial guess includes a number of trajectory points.

[0009] The target road is divided into regions according to the initial guess and the road shape of the target reference line to obtain a region division result; the region division result is a number of convex polygonal regions and a reference point corresponding to each of the convex polygonal regions; the target reference line is the reference line closest to the unmanned vehicle at the current moment; the road shape includes a straight line and an arc.

[0010] A multi-reference line penalty function is constructed according to the region division result.

[0011] An optimization-based trajectory smoothing model is constructed based on the multi-reference line penalty function.

[0012] The optimization-based trajectory smoothing model is solved using the initial guess as an initial iteration value to obtain a final trajectory of the unmanned vehicle.

[0013] In a second aspect, the present application provides an unmanned vehicle trajectory planning device, comprising the following modules.

[0014] The reference line information acquisition module is used to: acquire the reference line information of the target road of the unmanned vehicle; the reference line information includes main reference line information and secondary reference line information; the secondary reference line information is determined by the lateral offset between the secondary reference line and the main reference line.

[0015] The dynamic programming initial guess module is used to: perform dynamic programming according to the reference line information to obtain an initial guess of the driving trajectory of the unmanned vehicle; the initial guess includes a number of trajectory points.

[0016] The area division module is used to: divide the target road into areas according to the initial guess and the road shape of the target reference line to obtain an area division result; the area division result includes several convex polygonal areas and reference points corresponding to each convex polygonal area; the target reference line is the reference line closest to the unmanned vehicle at the current moment; the road shape includes a straight line and an arc.

[0017] The multi-reference line penalty function construction module is used to: construct a multi-reference line penalty function according to the area division result.

[0018] The optimized trajectory smoothing model construction module is used to construct an optimized trajectory smoothing model based on the multi-reference line penalty function.

[0019] The optimized trajectory smoothing model solving module is used to solve the optimized trajectory smoothing model with the initial guess as the initial iteration value to obtain the final trajectory of the unmanned vehicle.

[0020] In a third aspect, the present application provides a computer device, including: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the above-mentioned unmanned vehicle trajectory planning method.

[0021] In a fourth aspect, the present application provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the above-mentioned unmanned vehicle trajectory planning method.

[0022] According to the specific embodiments provided by the present application, the present application discloses the following technical effects: the present application provides a method, device, equipment and medium for trajectory planning of an unmanned vehicle, performs dynamic planning according to reference line information, obtains an initial guess of the driving trajectory of the unmanned vehicle, divides the target road into regions according to the initial guess and the road shape of the target reference line, obtains the regional division result, constructs a multi-reference line penalty function according to the regional division result, constructs an optimization-based trajectory smoothing model based on the multi-reference line penalty function, solves the optimization-based trajectory smoothing model with the initial guess as the initial iteration value, obtains the final trajectory of the unmanned vehicle, and can converge the lateral error between the trajectory and the reference line composed of a straight line and a curve. Regardless of whether it is a straight line or a curve, the trajectory can converge to the specified reference path in the horizontal direction, thereby minimizing the influence of the traditional model on the longitudinal optimization when optimizing the lateral error, and reducing the danger of unmanned driving. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings required for use in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0024] Figure 1 This is an application environment diagram of an unmanned vehicle trajectory planning method in one embodiment of the present application.

[0025] Figure 2 A flowchart of a method for unmanned vehicle trajectory planning provided in accordance with an embodiment of the present application.

[0026] Figure 3 A schematic diagram of reference lines and boundary lines provided for an embodiment of the present application.

[0027] Figure 4 A schematic diagram of an initial guess provided for an embodiment of the present application.

[0028] Figure 5 A schematic diagram of a convex polygonal area corresponding to a straight line segment provided in an embodiment of the present application.

[0029] Figure 6 A schematic diagram of a convex polygonal area corresponding to an arc section provided in an embodiment of the present application.

[0030] Figure 7A schematic diagram of the area division result provided in one embodiment of the present application.

[0031] Figure 8 A schematic diagram of the functional modules of an unmanned vehicle trajectory planning device provided in one embodiment of the present application.

[0032] Fig. 9 A schematic diagram of the structure of a computer device provided in one embodiment of the present application. DETAILED DESCRIPTION

[0033] The following will be combined with the drawings in the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0034] In order to make the above-mentioned objects, features and advantages of the present application more obvious and easy to understand, the present application is further described in detail below with reference to the accompanying drawings and specific implementation methods.

[0035] The unmanned vehicle trajectory planning method provided in the embodiment of the present application can be applied to Figure 1 In the application environment shown. Among them, the terminal 102 communicates with the server 104 through the network. The data storage system can store the data that the server 104 needs to process. The data storage system can be set up separately, integrated on the server 104, or placed on the cloud or other servers. The terminal 102 can send the reference line information of the target road to the server 104. After the server 104 receives the reference line information of the target road, for the reference line information of the target road, the server 104 performs dynamic planning based on the reference line information to obtain the initial guess of the driving trajectory of the unmanned vehicle, and divides the target road into regions based on the initial guess and the road shape of the target reference line to obtain the regional division result, and constructs a multi-reference line penalty function according to the regional division result. Based on the multi-reference line penalty function, a trajectory smoothing model based on optimization is constructed, and the initial guess is used as the initial iteration value. The trajectory smoothing model based on optimization is solved to obtain the final trajectory of the unmanned vehicle. The server 104 can feedback the final trajectory for the target road to the terminal 102. In addition, in some embodiments, the unmanned vehicle trajectory planning method can also be implemented independently by the server 104 or the terminal 102. For example, the terminal 102 can directly perform unmanned vehicle trajectory planning based on the reference line information, or the server 104 can obtain the reference line information of the target road from the data storage system and perform unmanned vehicle trajectory planning based on the reference line information.

[0036] The terminal 102 may be, but is not limited to, various desktop computers, laptop computers, smart phones, and tablet computers. The server 104 may be implemented as an independent server or a server cluster consisting of multiple servers, or may be a cloud server.

[0037] In an exemplary embodiment, Figure 2 As shown, a method for unmanned vehicle trajectory planning is provided. The method is executed by a computer device, and can be executed by a computer device such as a terminal or a server alone, or can be executed by a terminal and a server together. In the embodiment of the present application, the method is applied to Figure 1 The server 104 in the example is used for explanation, and the steps include the following steps 201 to 206.

[0038] Step 201: Acquire reference line information of a target road of an unmanned vehicle; the reference line information includes main reference line information and secondary reference line information; the secondary reference line information is determined by a lateral offset between the secondary reference line and the main reference line.

[0039] Step 202: Perform dynamic planning based on the reference line information to obtain an initial guess of the driving trajectory of the unmanned vehicle; the initial guess includes a number of trajectory points.

[0040] Step 203: Divide the target road into regions according to the initial guess and the road shape of the target reference line to obtain a region division result; the region division result includes a number of convex polygonal regions and a reference point corresponding to each of the convex polygonal regions; the target reference line is the reference line closest to the unmanned vehicle at the current moment; the road shape includes a straight line and an arc.

[0041] Step 204: construct a multi-reference line penalty function according to the region division result.

[0042] Step 205: constructing an optimization-based trajectory smoothing model based on the multi-reference line penalty function.

[0043] Step 206: Using the initial guess as the initial iteration value, solving the optimization-based trajectory smoothing model to obtain the final trajectory of the unmanned vehicle.

[0044] Implementing the above steps 201 to 206 can converge the lateral errors of the trajectory and the reference line consisting of a straight line and a curve. Whether on a straight line or a curve, the trajectory can converge to the specified reference path laterally, minimizing the impact of the traditional model on the longitudinal optimization when optimizing the lateral error. The present application can approximate multiple reference lines and reference lines of different shapes. The reference lines considered are not unique, such as urban roads with multiple lanes. Furthermore, a reference line may contain different shapes, such as straight segments and curved segments. Therefore, these reference line constraints exhibit a non-smooth "if-else" structure because they may come from different reference lines in different regions. This poses a challenge to trajectory optimization.

[0045] The main purpose of this application is to plan the trajectory of an unmanned vehicle in an environment with multiple reference line constraints, so the reference line information is given. It should be emphasized that multiple reference lines refer to the number of reference lines being greater than or equal to 1. The reference line information mainly includes the coordinate information of a main reference line, and all reference line information can be obtained based on the lateral offset of the main reference line.

[0046] In step 201, main reference line information is obtained from the global planning module, which is expressed in the form shown in formula (1).

[0047] (1).

[0048] in, Indicates the number of discrete points on the main reference line. Indicates The vertical coordinate of a discrete point in the Frenet coordinate system; Respectively represent The horizontal and vertical coordinate values ​​of discrete points in the global coordinate system; Indicates The heading angle of a discrete point. Indicates The radius of the curve where the discrete points are located, the curve turns left , turn right ; It should be noted that, , is the minimum turning radius of the unmanned vehicle, which is a basic parameter of the unmanned vehicle chassis. to identify straight road segments.

[0049] The reference lines on the target road other than the main reference line are named secondary reference lines. The set of lateral offsets of all reference lines and the main reference line is obtained from the global planning module and expressed as shown in formula (2).

[0050] (2).

[0051] in, Indicates the offset of all reference lines based on the main reference line. Indicates the number of The offset of the reference line, Indicates that the reference line is on the right side of the main reference line. Indicates that the reference line is on the left side of the main reference line. Take the 3.5m wide 3 lanes in the scene as an example. Indicates that the main reference line is in the middle lane. Indicates that the main reference line is the center line of the rightmost lane. Figure 3 As shown by the solid line in .

[0052] At a certain moment, the unmanned vehicle will only have one target reference line to approach, and the target reference line should also be the reference line closest to the unmanned vehicle at the current moment. In order to facilitate the judgment of which reference line the unmanned vehicle is closest to, we obtain the lateral offset of the boundary line of each reference line, expressed as shown in formula (3).

[0053] (3).

[0054] In the formula, +1 indicates the offset of the boundary line. Guides The offsets of the two corresponding boundary lines relative to the main reference line are , when the lateral offset of the unmanned vehicle satisfies When the target reference line of the unmanned vehicle is Reference lines. Taking the 3.5m wide 3-lane road as an example, the reference line offset set is , the boundary line offset set is , the boundary line is Figure 3 As shown by the dotted line in .

[0055] In another exemplary embodiment of the present application, step 202 specifically includes the following steps 301 to 303.

[0056] Step 301: Construct a cost function; the cost function includes a first cost associated with a reference line constraint and a second cost associated with a reference line constraint; wherein the first cost associated with the reference line constraint is used to constrain the unmanned vehicle to change lanes once within a single stage and prohibit continuous lane changes; the second cost associated with the reference line constraint is used to constrain the unmanned vehicle to only change the reference line once in two consecutive stages, and to make the unmanned vehicle travel along the target reference line for a set time after changing the reference line; the trajectory planning of the unmanned vehicle is divided into several stages.

[0057] Step 302: Using the cost function as a constraint condition, dynamic planning is performed according to the reference line information to obtain an initial guess of the driving trajectory of the unmanned vehicle.

[0058] This is done by establishing a Frenet coordinate system based on the reference line collected in step 201. Based on time, the decision-making problem of the unmanned vehicle is divided into The time interval between each stage is ,in, It’s the starting point of planning. is the termination time of the plan. The horizontal and vertical coordinates in the Frenet coordinate system represent the sampled state variables. ; and are the number of longitudinal and lateral samples in each stage respectively; the set of vehicle states allowed in each stage is determined by the minimum and maximum vehicle speeds and the maximum lateral speeds; the states of adjacent stages are connected by linear interpolation or polynomials, and the initial guess with the lowest cost is obtained by dynamic programming search. This implementation proposes a cost function related to the reference line constraint. The cost function includes the first cost related to the reference line constraint and the second cost related to the reference line constraint. The first cost related to the reference line constraint is shown in the following formula (4).

[0059] (4).

[0060] in, represents the first cost associated with the reference line constraint, is the weight, set by the user, indicating Indicates the lateral deviation between two adjacent lane lines; Indicates The effect of the first cost related to the reference line constraint is to allow the unmanned vehicle to change the reference line as soon as possible within a single stage, and it is forbidden to cross adjacent reference lines. In other words, the vehicle is only allowed to change lanes once within a single stage, and continuous lane changes are prohibited.

[0061] The second cost associated with the reference line constraint is expressed as shown in equation (5).

[0062] (5).

[0063] in, represents the second cost associated with the reference line constraint, is the weight, set by the user; Respectively represent -1 and +1 stage state variable lateral displacement; the second cost related to the reference line constraint is mainly to stipulate that the unmanned vehicle is only allowed to change the reference line once in two consecutive stages, so that the unmanned vehicle will try to drive along the target reference line for a period of time after changing the reference line.

[0064] The initial guess selected by the above cost function includes several trajectory points, and the initial guess is expressed as shown in formula (6).

[0065] (6).

[0066] in, They represent the initial guess The ordinate and abscissa of each trajectory point in the Frenet coordinate system; They represent the initial guess The horizontal and vertical coordinates of each trajectory point in the global coordinate system; is the number of initial guessed trajectory points. For example, Figure 4 As shown, Figure 4 The black dots in the figure are the trajectory points in the initial guess.

[0067] In another exemplary embodiment of the present application, step 203 may include the following steps 401 to 405.

[0068] Step 401: constructing a data structure of a target road and initializing the data structure to obtain an initialized data structure; the data structure is constructed based on the ordinates of the trajectory points in the initial guess; Step 402: The trajectory planning of the unmanned vehicle is divided into several stages; and it is determined whether the road shape of the target reference line corresponding to the current stage is a straight line or an arc.

[0069] Step 403: If the road shape of the target reference line corresponding to the current stage is a straight line, calculate the hyperplane parameters of each hyperplane according to the ordinates of the discrete points on the target reference line, the heading angles of the discrete points, and the coordinates of the discrete points on the boundary of the road where the target reference line is located; determine the convex polygon area according to the hyperplane parameters of each hyperplane; the convex polygon area is composed of multiple hyperplanes; and use the first trajectory of the straight section where the convex polygon area is located as the reference point corresponding to the convex polygon area.

[0070] Step 404: If the road shape of the target reference line corresponding to the current stage is an arc, calculate the hyperplane parameters of each hyperplane according to the longitudinal coordinates of the discrete points on the target reference line, the radius of the arc where the discrete points are located, and the coordinates of the discrete points on the boundary of the road where the target reference line is located; determine the convex polygon area according to the hyperplane parameters of each hyperplane; and use the center point of the target reference line as the reference point corresponding to the convex polygon area.

[0071] Step 405: traverse each stage to obtain the convex polygonal area corresponding to each stage and the reference point corresponding to each convex polygonal area.

[0072] Before partitioning, a new data structure is defined to facilitate explanation and use in subsequent steps. The data structure is expressed in the form shown in formula (7).

[0073] (7).

[0074] (8).

[0075] (9).

[0076] in, Represents a data structure. The default value is 0, which is determined by steps 403 and 404; Represents a convex polygon consisting of four hyperplanes in an absolute coordinate system. The four hyperplanes of the convex polygon can be determined by the parameters inside, that is, , four hyperplanes are determined by the four sets of parameters in formula (8), which are , , , ; Indicates the corresponding The reference point inside the enclosed convex polygonal area, are the horizontal and vertical coordinates of the reference point respectively. When the internal reference line is a straight line, is any point on the line, for example, it can be defined as the starting point of the line. Represents the heading angle of the straight line; when the convex polygon area When the internal reference line is a curve, are the coordinates of the center of the circle where the curve is located, Indicates the radius of the curve; The default value is 0. If It is still the default value, then the reference line inside the convex polygon area is a straight line.

[0077] Assuming the target reference line is a straight line, its lateral offset , the target reference line point set is expressed as shown in formula (10).

[0078] (10).

[0079] In the formula, Respectively The vertical coordinate of the discrete point in each stage in the Frenet coordinate system, the horizontal coordinate in the global coordinate system, the vertical coordinate in the global coordinate system, and the heading angle corresponding to the discrete point.

[0080] The vertical coordinates of discrete points on the target reference line in the Frenet coordinate system The corresponding main reference line coordinate ordered point set can be determined, expressed in the form shown in equation (11).

[0081] (11).

[0082] in, Indicates the number of points that constitute the main reference line of the straight segment, and the lateral offset of the left and right road boundaries of the straight segment road is known and , the coordinates of the left and right boundary lines in the global coordinate system can be obtained through the lateral offset. Take the first discrete point of the left boundary line and set it as , the first discrete point on the right boundary is set to , it is easy to see that the line connecting the two points is perpendicular to the main reference line, such as Figure 5 As shown. Based on this information, the convex polygon covering the straight road section can be determined. The convex polygon is expressed in the form shown in formula (8), that is, , and the first point of the main reference line of the straight section is expressed as shown in formula (9), that is,

[0083] Calculate the first hyperplane that passes through the first point ( ), is perpendicular to the main reference line of this segment, and needs to satisfy the form shown in the following formula (12).

[0084] (12).

[0085] The calculation formulas for the hyperplane parameters of the first hyperplane are shown in equations (13)-(15).

[0086] (13).

[0087] (14).

[0088] (15).

[0089] Calculate the second hyperplane that passes through the second point , perpendicular to the main reference line of this segment, and needs to satisfy the form shown in the following formula (16).

[0090] (16).

[0091] The calculation formulas for the hyperplane parameters of the second hyperplane are shown in equations (17)-(19).

[0092] (17).

[0093] (18).

[0094] (19).

[0095] Calculate the third hyperplane that passes through the third point ( ), parallel to the main reference line of this segment, and needs to satisfy the form shown in the following formula (20).

[0096] (20).

[0097] The calculation formulas for the hyperplane parameters of the third hyperplane are shown in equations (21)-(23).

[0098] (twenty one).

[0099] (twenty two).

[0100] (twenty three).

[0101] Calculate the fourth hyperplane, which passes through the fourth point , parallel to the main reference line of this segment, and needs to satisfy the form shown in the following formula (24).

[0102] (twenty four).

[0103] The calculation formulas for the hyperplane parameters of the fourth hyperplane are shown in equations (25)-(27).

[0104] (25).

[0105] (26).

[0106] (27).

[0107] in, is the hyperplane parameter of the first hyperplane, are the horizontal and vertical coordinates of the first point in the global coordinate system; is the hyperplane parameter of the second hyperplane; are the horizontal and vertical coordinates of the second point in the global coordinate system; is the hyperplane parameter of the third hyperplane; are the horizontal and vertical coordinates of the third point in the global coordinate system; is the hyperplane parameter of the fourth hyperplane; are the horizontal and vertical coordinates of the fourth point in the global coordinate system. The convex polygon area corresponding to the straight line segment is as follows Figure 5 Shown as thick solid line.

[0108] Assuming the target reference line is an arc, its lateral offset , the target reference line point set is expressed as shown in formula (28).

[0109] (28).

[0110] Through the target reference line The corresponding main reference line coordinate ordered point set can be easily expressed as shown in formula (29).

[0111] (29).

[0112] in, Indicates the number of points that constitute the main reference line of the arc segment, and the lateral offset of the left and right road boundaries of the arc segment road is known and , the coordinates of the left and right boundary lines in the global coordinate system can be obtained through the lateral offset. Three discrete points are taken on the left and right boundary lines: the first point, the middle point and the last point. The coordinates of the three discrete points corresponding to the left and right boundary lines are set as , , , , , ,like Figure 6 As shown. Based on this information, the convex polygon covering the straight road section can be determined. The convex polygon is expressed in the form shown in formula (8), that is, , and the target reference line center point of the arc is expressed as shown in formula (9), that is, .

[0113] when , calculate the first hyperplane, which passes through the point , , and needs to satisfy the form shown in the following formula (30).

[0114] (30).

[0115] The calculation formulas for the hyperplane parameters of the first hyperplane are shown in equations (31)-(33).

[0116] (31).

[0117] (31).

[0118] (33).

[0119] Calculate the second hyperplane that passes through the point , , and needs to satisfy the form shown in the following formula (34).

[0120] (34).

[0121] The calculation formulas for the hyperplane parameters of the second hyperplane are shown in equations (35)-(37).

[0122] (35).

[0123] (36).

[0124] (37).

[0125] Calculate the third hyperplane, which passes through the point , , and needs to satisfy the form shown in the following formula (38).

[0126] (38).

[0127] The calculation formulas for the hyperplane parameters of the third hyperplane are shown in equations (39)-(41).

[0128] (39).

[0129] (40).

[0130] (41).

[0131] Calculate the fourth hyperplane, which passes through the point , perpendicular to the vector , and needs to satisfy the form shown in the following formula (42).

[0132] (42).

[0133] The calculation formulas for the hyperplane parameters of the fourth hyperplane are shown in equations (43)-(45).

[0134] (43).

[0135] (44).

[0136] (45).

[0137] Similarly, when , swap the three points of the left and right boundary lines based on the above. The convex polygon area corresponding to the arc section is as follows Figure 6 Shown as thick solid line.

[0138] Traversal The lateral offset in ,judge and The real number interval cover formed Real number elements in: If covered, it means the first The trajectory points and The trajectory formed by the trajectory points is changing the reference line. At this time, the parameter , and get two index values and . Using two index values, the first Track point to The trajectory composed of all the trajectory points in the trajectory points is considered to be the trajectory of the reference line replacement process. You can index the trajectory points corresponding to the main reference line corresponding to a certain trajectory point in the initial guess. Mark the pairs with equal The number of stages of changing the reference line is assumed to be , the set of reference line intervals can be expressed as shown in formula (46).

[0139] (46).

[0140] Step 3.3 Divide the road according to the initial guess According to the initial guess, the road is divided into regions. First, a data structure as shown in formula (7) is established and initialized. The initialized data structure is expressed in the form shown in formula (47).

[0141] (47).

[0142] make , which means that the initial guess The longitudinal displacement in is copied to The initial value of each longitudinal displacement is 0, indicating

[0143] After processing initialization , eliminating the corresponding phase of changing the reference line , after processing Satisfy the constraints shown in formula (48). (48).

[0144] After this proposal, Only lane keeping is included.

[0145] The following is the processed Divide the road shape in turn: if it is a straight line, divide it using the method provided in step 403 to obtain the corresponding , and at the same time If it is an arc, the method provided in step 404 is used to divide it and obtain the corresponding , and at the same time Add 1.

[0146] Finally, the regional division results can be obtained , the division result includes several convex polygonal areas and the reference points corresponding to each convex polygonal area. Figure 4 Take the road situation shown in the figure as an example, and divide the road into regions. The regional division results are as follows Figure 7 shown.

[0147] When the driverless car enters the convex polygon area , the unmanned vehicle will be required to approach the reference line corresponding to the convex polygon area. right The constraint is defined as shown in formula (49).

[0148] (49).

[0149] when = 0, for any , are all 0, that is, the state at this time constraint.

[0150] right Definition and convex polygon area The correlation is expressed as shown in formula (50).

[0151] (50).

[0152] right There are two cases for the definition of When the interior is a straight section, It is expressed as shown in formula (51).

[0153] (51).

[0154] When the convex polygon area When the interior is an arc section, It is expressed as the following formula.

[0155] (52).

[0156] in, represents the multi-reference line penalty function; Indicates The first penalty part corresponding to the convex polygon area; Indicates The second penalty part corresponding to the convex polygon area; , , , are the hyperplane parameters of the hyperplane that constitutes the convex polygon area; represents the coordinates of the trajectory points in the convex polygon region, is the horizontal axis, is the vertical coordinate; when the convex polygon area When the internal reference line is a straight line, , , are the horizontal coordinate and vertical coordinate heading angle of the position point on the reference line (all heading angles on the straight line are equal); when the convex polygon area When the road shape of the internal reference line is an arc, and are the horizontal and vertical coordinates of the center of the arc reference line, The radius of the arc reference line corresponding to the center of the circle.

[0157] In order to alleviate the non-differentiability, this embodiment adopts a smooth logsumexp function approximation, and the approximation process of the logsumexp function is shown in the following equations (53) and (54).

[0158] (53).

[0159] (54).

[0160] in, Represents the logsumexp function.

[0161] For all convex polygonal areas and the reference points corresponding to each convex polygonal area , which can be expressed as formula (55).

[0162] (55).

[0163] Since the convex polygon area above are disjoint, so for any point , at most one is activated, are disjoint with each other and can be expressed as shown in formula (56).

[0164] (56).

[0165] The multi-reference line penalty function can be added to many existing nonlinear trajectory optimization models. The trajectory smoothing model based on optimization includes an objective function and constraints; the constraints include vehicle kinematics or dynamic model constraints of the unmanned vehicle, amplitude constraints of the state vector, amplitude constraints of the control vector, two-point boundary value constraints, and obstacle avoidance constraints. In another exemplary embodiment of the present application, the trajectory smoothing model based on optimization is expressed in the form shown in formula (57).

[0166] (57).

[0167] in, represents the objective function of the optimization-based trajectory smoothing model, represents the weight coefficient, for Time acceleration The first derivative of for Front wheel deflection angle of the unmanned vehicle at any moment The first derivative of represents the multi-reference line penalty function, Represents the vehicle kinematics or dynamics model constraints of the unmanned vehicle, represents the state space equation, express State vector at the moment , They are the horizontal coordinate, vertical coordinate, heading angle, speed, front wheel deflection angle and acceleration of the unmanned vehicle respectively; for The state vector at time, for The control vector at time, Represents the control vector ; represents the magnitude constraint of the state vector, are the lower and upper limits of the state vector respectively; represents the magnitude constraint of the control vector, are the lower and upper limits of the control vector respectively; is a two-point boundary value constraint, are the state vector at the starting time and the set value of the state vector at the starting time respectively; are the terminal state vector and the set value of the terminal state vector, respectively. Indicates the terminal time; To avoid obstacle constraints, Indicates a collision-free area.

[0168] The initial guess is used as the iterative initial value of the trajectory smoothing model based on optimization, and the above-mentioned trajectory smoothing model based on optimization is iteratively solved using a numerical method to obtain the final trajectory.

[0169] The present application also provides an application scenario, which applies the above-mentioned unmanned vehicle trajectory planning method. Specifically: the unmanned vehicle trajectory planning method provided in this embodiment can be applied in an unmanned driving scenario. The unmanned driving scenario includes an information acquisition link, a trajectory planning link and an unmanned driving link; the reference line information of the target road enters the trajectory planning link from the information acquisition link, obtains the corresponding final trajectory, and enters the downstream unmanned driving link. The unmanned vehicle trajectory planning method provided in this embodiment belongs to the trajectory planning link. Specifically, in the trajectory planning process for the target road, dynamic planning can be performed according to the reference line information to obtain an initial guess of the unmanned vehicle's driving trajectory, and the target road is divided into regions according to the initial guess and the road shape of the target reference line to obtain a regional division result, and a multi-reference line penalty function is constructed according to the regional division result. Based on the multi-reference line penalty function, an optimization-based trajectory smoothing model is constructed, and the initial guess is used as the initial iteration value. The optimization-based trajectory smoothing model is solved to obtain the final trajectory of the unmanned vehicle.

[0170] Based on the same inventive concept, the embodiment of the present application also provides an unmanned vehicle trajectory planning device for implementing the unmanned vehicle trajectory planning method involved above. The implementation scheme for solving the problem provided by the device is similar to the implementation scheme recorded in the above method, so the specific limitations in one or more unmanned vehicle trajectory planning device embodiments provided below can refer to the limitations of the unmanned vehicle trajectory planning method above, and will not be repeated here.

[0171] In an exemplary embodiment, Figure 8 As shown, an unmanned vehicle trajectory planning device is provided, including the following modules.

[0172] The reference line information acquisition module T1 is used to: acquire the reference line information of the target road of the unmanned vehicle; the reference line information includes main reference line information and secondary reference line information; the secondary reference line information is determined by the lateral offset between the secondary reference line and the main reference line.

[0173] The dynamic planning initial guess module T2 is used to: perform dynamic planning according to the reference line information to obtain an initial guess of the driving trajectory of the unmanned vehicle; the initial guess includes a number of trajectory points.

[0174] The area division module T3 is used to: divide the target road into areas according to the initial guess and the road shape of the target reference line to obtain an area division result; the area division result includes several convex polygonal areas and reference points corresponding to each convex polygonal area; the target reference line is the reference line closest to the unmanned vehicle at the current moment; the road shape includes a straight line and an arc.

[0175] The multi-reference line penalty function construction module T4 is used to: construct a multi-reference line penalty function according to the region division result.

[0176] The optimized trajectory smoothing model construction module T5 is used to construct an optimized trajectory smoothing model based on the multi-reference line penalty function.

[0177] The optimized trajectory smoothing model solving module T6 is used to solve the optimized trajectory smoothing model with the initial guess as the initial iteration value to obtain the final trajectory of the unmanned vehicle.

[0178] In an exemplary embodiment, a computer device is provided. The computer device may be a server or a terminal. The internal structure diagram thereof may be as follows: Fig. 9 As shown. The computer device includes a processor, a memory, an input / output interface (Input / Output, referred to as I / O) and a communication interface. Among them, the processor, the memory and the input / output interface are connected through a system bus, and the communication interface is connected to the system bus through the input / output interface. Among them, the processor of the computer device is used to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system, a computer program and a database. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The database of the computer device is used to store unmanned vehicle trajectory planning data. The input / output interface of the computer device is used to exchange information between the processor and an external device. The communication interface of the computer device is used to communicate with an external terminal through a network connection. When the computer program is executed by the processor, a method for unmanned vehicle trajectory planning is implemented.

[0179] Those skilled in the art will understand that Fig. 9 The structure shown in the figure is only a block diagram of a part of the structure related to the solution of the present application, and does not constitute a limitation on the computer device to which the solution of the present application is applied. The specific computer device may include more or fewer components than those shown in the figure, or combine certain components, or have a different arrangement of components.

[0180] In an exemplary embodiment, a computer device is further provided, including a memory and a processor, wherein a computer program is stored in the memory, and the processor implements the steps in the above-mentioned method embodiments when executing the computer program.

[0181] In an exemplary embodiment, a computer-readable storage medium is provided, storing a computer program, and when the computer program is executed by a processor, the steps in the above method embodiments are implemented.

[0182] It should be noted that the user information (including but not limited to user device information, user personal information, etc.) and data (including but not limited to data used for analysis, stored data, displayed data, etc.) involved in this application are all information and data authorized by the user or fully authorized by all parties, and the collection, use and processing of relevant data must comply with relevant regulations.

[0183] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing the relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to the memory, database or other medium used in the embodiments provided in this application can include at least one of non-volatile and volatile memory. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory, optical memory, high-density embedded non-volatile memory, resistive random access memory (ReRAM), magnetoresistive random access memory (MRAM), ferroelectric random access memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory can include random access memory (RAM) or external cache memory, etc. By way of illustration and not limitation, RAM may be in various forms, such as Static Random Access Memory (SRAM) or Dynamic Random Access Memory (DRAM).

[0184] The database involved in each embodiment provided in this application may include at least one of a relational database and a non-relational database. The non-relational database may include a distributed database based on blockchain, etc., but is not limited thereto. The processor involved in each embodiment provided in this application may be a general-purpose processor, a central processing unit, a graphics processor, a digital signal processor, a programmable logic device, a data processing logic device based on quantum computing, etc., but is not limited thereto.

[0185] The technical features of the above embodiments may be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0186] This article uses specific examples to illustrate the principles and implementation methods of this application. The description of the above embodiments is only used to help understand the method and core ideas of this application. At the same time, for those skilled in the art, according to the ideas of this application, there will be changes in the specific implementation methods and application scope. In summary, the content of this specification should not be understood as limiting this application.

Claims

1. A method for unmanned vehicle trajectory planning, characterized in that: The unmanned vehicle trajectory planning method comprises: Acquire reference line information of a target road of the unmanned vehicle; the reference line information includes main reference line information and secondary reference line information; the secondary reference line information is determined by a lateral offset between the secondary reference line and the main reference line; Perform dynamic planning based on the reference line information to obtain an initial guess of the driving trajectory of the unmanned vehicle; the initial guess includes a number of trajectory points; According to the initial guess and the road shape of the target reference line, the target road is divided into regions to obtain a region division result; the region division result is a number of convex polygonal regions and a reference point corresponding to each of the convex polygonal regions; the target reference line is the reference line closest to the unmanned vehicle at the current moment; the road shape includes a straight line and an arc; Constructing a multi-reference line penalty function according to the region division result; constructing an optimization-based trajectory smoothing model based on the multi-reference line penalty function; The optimization-based trajectory smoothing model is solved using the initial guess as an initial iteration value to obtain a final trajectory of the unmanned vehicle.

2. The unmanned vehicle trajectory planning method according to claim 1, characterized in that: Dynamic planning is performed based on the reference line information to obtain an initial guess of the unmanned vehicle's driving trajectory, including: Constructing a cost function; the cost function includes a first cost associated with the reference line constraint and a second cost associated with the reference line constraint; wherein the first cost associated with the reference line constraint is used to constrain the unmanned vehicle to change lanes once within a single stage time and prohibit continuous lane changes; the second cost associated with the reference line constraint is used to constrain the unmanned vehicle to only change the reference line once in two consecutive stages, and to make the unmanned vehicle drive along the target reference line for a set time after changing the reference line; the trajectory planning of the unmanned vehicle is divided into several stages; Taking the cost function as a constraint condition, dynamic planning is performed according to the reference line information to obtain an initial guess of the driving trajectory of the unmanned vehicle.

3. The unmanned vehicle trajectory planning method according to claim 1, characterized in that: The target road is divided into regions according to the initial guess and the road shape of the target reference line to obtain a region division result, which specifically includes: Constructing a data structure of the target road and initializing the data structure to obtain an initialized data structure; the data structure is constructed based on the ordinates of the trajectory points in the initial guess; The trajectory planning of the unmanned vehicle is divided into several stages; it is determined whether the road shape of the target reference line corresponding to the current stage is a straight line or an arc; If the road shape of the target reference line corresponding to the current stage is a straight line, the hyperplane parameters of each hyperplane are calculated according to the ordinates of the discrete points on the target reference line, the heading angles of the discrete points, and the coordinates of the discrete points on the boundary of the road where the target reference line is located; the convex polygon area is determined according to the hyperplane parameters of each hyperplane; the convex polygon area is composed of multiple hyperplanes; the first trajectory of the straight section where the convex polygon area is located is used as the reference point corresponding to the convex polygon area; If the road shape of the target reference line corresponding to the current stage is an arc, the hyperplane parameters of each hyperplane are calculated according to the ordinates of the discrete points on the target reference line, the radius of the arc where the discrete points are located, and the coordinates of the discrete points on the boundary of the road where the target reference line is located; the convex polygon area is determined according to the hyperplane parameters of each hyperplane; and the center point of the target reference line is used as the reference point corresponding to the convex polygon area; Each stage is traversed to obtain the convex polygonal area corresponding to each stage and the reference point corresponding to each convex polygonal area.

4. The unmanned vehicle trajectory planning method according to claim 1, characterized in that: The calculation formula of the multi-reference line penalty function is as follows: ； ； When the convex polygon area When the interior is a straight section, It is expressed as: ； When the convex polygon area When the interior is an arc section, It is expressed as: ； in, represents the multi-reference line penalty function; Indicates The first penalty part corresponding to the convex polygon area; Indicates The second penalty part corresponding to the convex polygon area; , , , are the hyperplane parameters of the hyperplane that constitutes the convex polygon area; represents the coordinates of the trajectory points in the convex polygon region, is the horizontal axis, is the vertical coordinate; when the convex polygon area When the internal reference line is a straight line, , , are the horizontal coordinate and vertical coordinate heading angle of the position point on the reference line respectively; when the convex polygon area When the road shape of the internal reference line is an arc, and are the horizontal and vertical coordinates of the center of the arc reference line, The radius of the arc reference line corresponding to the center of the circle.

5. The unmanned vehicle trajectory planning method according to claim 1, characterized in that: The optimization-based trajectory smoothing model includes an objective function and constraints; the constraints include vehicle kinematics or dynamics model constraints of the unmanned vehicle, amplitude constraints of the state vector, amplitude constraints of the control vector, two-point boundary value constraints, and obstacle avoidance constraints.

6. The unmanned vehicle trajectory planning method according to claim 1, characterized in that: The optimization-based trajectory smoothing model is expressed as follows: ； in, represents the objective function of the optimization-based trajectory smoothing model, represents the weight coefficient; for Time acceleration The first derivative of for Front wheel deflection angle of the unmanned vehicle at any moment The first derivative of represents the multi-reference line penalty function, Represents the vehicle kinematics or dynamics model constraints of the unmanned vehicle, represents the state space equation, express The state vector at time , They are the horizontal coordinate, vertical coordinate, heading angle, speed, front wheel deflection angle and acceleration of the unmanned vehicle respectively; for The state vector at time, for The control vector at time, Represents the control vector ; represents the magnitude constraint of the state vector, are the lower and upper limits of the state vector respectively; represents the magnitude constraint of the control vector, are the lower and upper limits of the control vector respectively; is a two-point boundary value constraint, are the state vector at the starting time and the set value of the state vector at the starting time respectively; are the terminal state vector and the set value of the terminal state vector, respectively. Indicates the terminal moment; To avoid obstacles, Indicates a collision-free area.

7. An unmanned vehicle trajectory planning device, characterized in that: The unmanned vehicle trajectory planning device comprises: A reference line information acquisition module is used to: acquire reference line information of a target road of the unmanned vehicle; the reference line information includes main reference line information and secondary reference line information; the secondary reference line information is determined by a lateral offset between the secondary reference line and the main reference line; The dynamic planning initial guess module is used to: perform dynamic planning according to the reference line information to obtain an initial guess of the driving trajectory of the unmanned vehicle; the initial guess includes a number of trajectory points; A region division module is used to: divide the target road into regions according to the initial guess and the road shape of the target reference line to obtain a region division result; the region division result includes a number of convex polygonal regions and a reference point corresponding to each of the convex polygonal regions; the target reference line is the reference line closest to the unmanned vehicle at the current moment; the road shape includes a straight line and an arc; A multi-reference line penalty function construction module is used to: construct a multi-reference line penalty function according to the area division result; An optimized trajectory smoothing model construction module is used to: construct an optimized trajectory smoothing model based on the multi-reference line penalty function; The optimized trajectory smoothing model solving module is used to solve the optimized trajectory smoothing model with the initial guess as the initial iteration value to obtain the final trajectory of the unmanned vehicle.

8. A computer device comprising: A memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the unmanned vehicle trajectory planning method according to any one of claims 1 to 6.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the unmanned vehicle trajectory planning method described in any one of claims 1 to 6 is implemented.

Images