A constraint iteration-based LQR motion planning method, device, medium and product

Abstract

The application discloses an LQR motion planning method and device based on constraint iteration, a medium and a product, relates to the technical field of unmanned vehicle motion planning, and comprises the following steps: constructing a convex space boundary and a reference line according to the position of an obstacle in a lane; and performing trajectory planning on a vehicle to be planned based on the reference line. The process of trajectory planning comprises the following steps: acquiring the number of CILQR planning points and initial values of state variables and control variables of the vehicle to be planned. The state variables comprise a horizontal coordinate, a vertical coordinate, a heading angle, a speed and an acceleration. The control variables comprise an acceleration change rate and a yaw rate. Based on the initial values of the state variables and the control variables of the vehicle to be planned, the state variables and the control variables corresponding to each CILQR planning point are updated and iterated through multiple rounds by using back propagation and forward propagation, so that a target trajectory of the vehicle to be planned is obtained, and trajectory planning is completed. The application can achieve good planning effect in a dynamic scene, and improves the safety of obstacle avoidance trajectory planning.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of unmanned vehicle motion planning, in particular to an LQR motion planning method and device based on constraint iteration, a medium and a product. BACKGROUND

[0002] The motion planning of an unmanned vehicle is connected with environment perception and positioning at the top and vehicle control at the bottom, and its purpose is to output a smooth and collision-free trajectory that meets the kinematic and dynamic characteristics of the vehicle. Motion planning can be divided into path planning and trajectory planning. Path planning is a sequence of positions connecting the starting point and the target point, only considering the position in space, while trajectory planning adds time sequence information to each point, giving it speed and acceleration information, so that it meets the kinematic constraints of the vehicle and facilitates the effect of control tracking.

[0003] Global planning generally only needs to perform path planning and does not need to contain time information. Common global path planning methods include Dijkstra algorithm, A* algorithm and RRT algorithm. The purpose of global planning is to output a reference trajectory to facilitate further local obstacle avoidance trajectory planning. The relatively mature local trajectory planning method in recent years is the curve interpolation method, which selects the starting point, the ending point and a number of intermediate points as control points, and uses Bezier and polynomial curves to interpolate to generate a series of trajectory points. The disadvantage is that the output result depends on the selection of control points, and it has local characteristics and is not reliable in areas far from the control points. In recent years, Apollo has proposed a sampling-based obstacle avoidance trajectory planning method, which uniformly samples discrete points near the reference trajectory, and solves them using a quadratic programming solver according to the cost function and constraint conditions. The disadvantage is that the quadratic programming solver cannot directly solve the problem containing nonlinear constraints, such as curvature constraints, and the sampling method has high time complexity, which is difficult to meet the real-time planning requirements. SUMMARY

[0004] The purpose of the present application is to provide an LQR motion planning method and device based on constraint iteration, a medium and a product, which can achieve good planning effect in dynamic scenes and improve the safety of obstacle avoidance trajectory planning.

[0005] To achieve the above purpose, the present application provides the following solutions.

[0006] In a first aspect, the present application provides an LQR motion planning method based on constraint iteration, comprising:

[0007] constructing a convex space boundary and a reference line according to the position of the obstacle in the lane;

[0008] performing trajectory planning for the vehicle to be planned based on the reference line within the convex space boundary; the process of trajectory planning comprises:

[0009] obtaining a number of CILQR planning points and initial values of state variables and control variables of the vehicle to be planned; the state variables include: a horizontal coordinate, a vertical coordinate, a heading angle, a speed and an acceleration; the control variables include: an acceleration change rate and a yaw rate;

[0010] based on the initial values of the state variables and the control variables of the vehicle to be planned, performing multiple rounds of update iterations on the state variables and the control variables corresponding to each CILQR planning point by using back propagation and forward propagation, to obtain a target trajectory of the vehicle to be planned, and completing trajectory planning.

[0011] Optionally, based on the initial values of the state variables and the control variables of the vehicle to be planned, performing multiple rounds of update iterations on the state variables and the control variables corresponding to each CILQR planning point by using back propagation and forward propagation, to obtain a target trajectory of the vehicle to be planned, and completing trajectory planning, comprising:

[0012] determining any iteration number greater than 1 as a current iteration number;

[0013] determining the control variables corresponding to each CILQR planning point in the trajectory at the current iteration number; the control variables corresponding to each CILQR planning point in the trajectory at the current iteration number are determined by using back propagation based on the control variables corresponding to each CILQR planning point in the trajectory at the last iteration number; and the control variables corresponding to each CILQR planning point in the trajectory at the initial iteration number are determined by initialization;

[0014] determining the state variables corresponding to each CILQR planning point in the trajectory at the current iteration number by using forward propagation according to the initial values of the state variables of the vehicle to be planned and the initial values of the control variables of the starting point at the current iteration number;

[0015] calculating a total cost of the trajectory at the current iteration number based on the state variables and the control variables corresponding to each CILQR planning point in the trajectory at the current iteration number and the state variables and the control variables corresponding to each point in the reference line by using a total cost function; the total cost function is a function of the control variables and the state variables;

[0016] calculating a cost difference at the current iteration number based on the total cost of the trajectory at the current iteration number and the total cost of the trajectory at the last iteration number;

[0017] determining whether a stop condition is met; the stop condition is that the cost difference at the current iteration number is less than a preset difference value or a preset iteration number is reached;

[0018] if yes, determining the trajectory at the current iteration number as the target trajectory of the vehicle to be planned;

[0019] If no, the current iteration number is updated as the next iteration number, and the method returns to "determining the control amount corresponding to each CILQR planning point in the trajectory at the current iteration number", until the stop condition is met, and the target trajectory of the vehicle to be planned is obtained.

[0020] Optionally, the total cost function comprises an obstacle distance cost function, a reference line distance cost function, a convex space boundary distance cost function and a control amount cost function.

[0021] Optionally, the obstacle distance cost function is:

[0022] J obs =q1×exp(q2(X-X0 T P(X-X0));

[0023] wherein J obs is an obstacle distance cost function value; q1 and q2 are weight coefficients of the obstacle distance cost function; X is a matrix composed of state amounts corresponding to each CILQR planning point in the trajectory; X0 is a matrix composed of state amounts corresponding to each point in the obstacle; T is a transpose; and P is a matrix containing information of long and short axes of an ellipse.

[0024] Optionally, the reference line distance cost function is:

[0025] J ref =(X-X ref ) T p1(X-X ref );

[0026] wherein J ref is a reference line distance cost function value; x ref is a matrix composed of state amounts corresponding to each point in the reference line; and p1 is a weight coefficient of the reference line distance cost function.

[0027] Optionally, the convex space boundary distance cost function is:

[0028] J boundary =q3×exp(q4(X-X boundary ));

[0029] wherein J boundary is a convex space boundary distance cost function value; q3 and q4 are weight coefficients of the convex space boundary distance cost function; and X boundary is a matrix composed of state amounts corresponding to projection points of each CILQR planning point in the trajectory on the convex space boundary.

[0030] Optionally, the control amount cost function is:

[0031] J controlq5*exp(q6(U-U max ))+q7*exp(q8(U min -U));

[0032] wherein, J control is the control cost function value; q5, q6 and q7 are weight coefficients of the control cost function; U is a matrix composed of control quantities corresponding to each CILQR planning point in the trajectory; U max is the maximum value in the matrix composed of control quantities corresponding to each CILQR planning point in the trajectory; U min is the minimum value in the matrix composed of control quantities corresponding to each CILQR planning point in the trajectory.

[0033] In a second aspect, the present application provides 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 constraint iteration-based LQR motion planning method according to any one of the preceding aspects.

[0034] In a third aspect, the present application provides a computer readable storage medium having a computer program stored thereon, wherein the computer program is executable by a processor to implement the constraint iteration-based LQR motion planning method according to any one of the preceding aspects.

[0035] In a fourth aspect, the present application provides a computer program product comprising a computer program, wherein the computer program is executable by a processor to implement the constraint iteration-based LQR motion planning method according to any one of the preceding aspects.

[0036] According to the embodiments provided in the present application, the following technical effects are disclosed:

[0037] The present application discloses a constraint iteration-based LQR motion planning method, device, medium and product, constructs a convex space boundary, facilitates CILQR to solve within the range of the convex space boundary, prevents different frames from having different decision results, and ensures the uniqueness of the solution; planning is performed in space and time, which can achieve good planning effect in a dynamic scene and improve the safety of obstacle avoidance trajectory planning. BRIEF DESCRIPTION OF DRAWINGS

[0038] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the drawings needed in the embodiments will be briefly introduced below. Obviously, the drawings in the following description are only some embodiments of the present application, and for those skilled in the art, other drawings can also be obtained without creative labor on the basis of these drawings.

[0039] Figure 1A flowchart of a LQR motion planning method based on constraint iteration provided by an embodiment of the present application is shown in FIG. 1.

[0040] Figure 2 A flowchart of an obstacle avoidance trajectory optimization algorithm based on CILQR is shown in FIG. 2.

[0041] Figure 3 A schematic diagram of a bisection method for calculating the projection point of an obstacle on a lane line is shown in FIG. 3.

[0042] Figure 4 A schematic diagram of an obstacle avoidance convex space generated when changing lanes to the right in a three-lane scenario is shown in FIG. 4.

[0043] Figure 5 A schematic diagram of an obstacle avoidance convex space generated when straight driving in a three-lane scenario is shown in FIG. 5.

[0044] Figure 6 A schematic diagram of a trajectory planning result of CILQR in a lane changing scenario is shown in FIG. 6.

[0045] Figure 7 A schematic diagram of the structure of a computer device provided by an embodiment of the present application is shown in FIG. 7. DETAILED DESCRIPTION

[0046] The technical solutions in the embodiments of the present application will be described clearly and completely below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are only some of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative work fall within the scope of the present application.

[0047] The purpose of the present application is to provide a LQR motion planning method, device, medium and product based on constraint iteration, aiming to improve the safety of obstacle avoidance trajectory planning.

[0048] In order to make the above-mentioned purposes, features and advantages of the present application more obvious and easy to understand, the present application will be further described in detail below with reference to the drawings and specific embodiments.

[0049] In an exemplary embodiment, as shown in FIGS. 1 and 2, the LQR motion planning method based on constraint iteration in the present embodiment includes the following steps. Figure 1 and Figure 2

[0050] Step 1: Construct the convex space boundary and reference line according to the position of the obstacle in the lane.

[0051] Specifically, first, the four corner points of the obstacle are constructed according to the center point position and size of the obstacle. The projection point of each corner point on the lane line is found, and it is determined in which lane the obstacle is located. In order to reduce the time consumption, the bisection method is used to find the nearest neighbor point, as shown in FIG. 3. Figure 3 ​as shown. Figure 3 wherein, θ is the heading angle.

[0052] (1) When there is no obstacle within 100m from the ego vehicle (i.e. the target vehicle) in the lane where the ego vehicle is located, the lane change intention is straight driving, and each point on the reference line is uniformly sampled N points of the center line of the current lane (i.e. the lane where the ego vehicle is located) within the CILQR planning length range. N is the number of CILQR planning points.

[0053] (2) When the obstacle is located in the lane where the ego vehicle is located, and there is no obstacle within 100m in the left lane, the lane change intention is left lane change, and each point on the reference line is uniformly sampled N points of the center line of the left lane within the CILQR planning length range.

[0054] (3) When the obstacle is located in the lane where the ego vehicle is located, and there is an obstacle within 100m in the left lane, and there is no obstacle within 100m in the right lane, the lane change intention is right lane change, and each point on the reference line is uniformly sampled N points of the center line of the right lane within the CILQR planning length range.

[0055] (4) When the obstacle is located in the lane where the ego vehicle is located, and there is an obstacle within 100m in the left and right lanes, the lane change intention is straight driving, and each point on the reference line is uniformly sampled N points of the center line of the current lane within the range of the smaller value between the CILQR planning length and the length between the ego vehicle and the obstacle.

[0056] Step 2: Within the convex space boundary, trajectory planning is performed for the vehicle to be planned based on the reference line.

[0057] The process of trajectory planning includes:

[0058] Step 21: Obtain the number of CILQR planning points and the initial values of the state variables and control variables of the vehicle to be planned. The state variables include: horizontal coordinate, vertical coordinate, heading angle, speed and acceleration. The control variables include: acceleration rate and yaw rate.

[0059] Step 22: Based on the initial values of the state variables and control variables of the vehicle to be planned, the state variables and control variables corresponding to each CILQR planning point are updated and iterated multiple times using back propagation and forward propagation, to obtain the target trajectory of the vehicle to be planned, and complete the trajectory planning.

[0060] As an optional implementation, step 22 includes:

[0061] Step 221: Determine any iteration number greater than 1 as the current iteration number.

[0062] Step 222: determining the control quantity corresponding to each CILQR planning point in the trajectory at the current iteration number; the control quantity corresponding to each CILQR planning point in the trajectory at the current iteration number is determined by using back propagation based on the control quantity corresponding to each CILQR planning point in the trajectory at the last iteration number; and the control quantity corresponding to each CILQR planning point in the trajectory at the initial iteration number is determined by initialization.

[0063] Specifically, the process of determining the control quantity corresponding to each CILQR planning point by using back propagation includes:

[0064] (1) To minimize the objective function value, define the value function V and the action function Q of each CILQR planning point:

[0065] Q t (x t ,u t )=L(x t ,u t )+V t+1 (x t+1 )t=0,1,2,…,N-1。

[0066] V t (x t )=minQ t (x t ,u t )t=0,1,2,…,N-1。

[0067] V N (x N )=L f (x N )。

[0068] x t+1 =f(x t ,u t )。

[0069]

[0070] Wherein, Q t (x t ,u t ) is the action function value of the tthCILQR planning point; x t is the state quantity of the tthCILQR planning point; u t is the control quantity of the tthCILQR planning point; L(x t ,u t ) is the cost function value of the tthCILQR planning point; V t+1 (x t+1 ) is the value function value of the t+1thCILQR planning point; x t+1Vt+1is the state quantity of the t+1th CILQR planning point; t t Vtis the value function value of the tth CILQR planning point; N N VNis the value function value of the Nth CILQR planning point; N xNis the state quantity of the Nth CILQR planning point; f N f(xN, uN) is the cost function value of the Nth CILQR planning point; t t f(x, u) is a function about x and u; t t x0is the state quantity of the initial CILQR planning point; start

[0071] The problem can be converted to finding new x t t , u at each time step (i.e. iteration number) to minimize Q t (x t , u t ).

[0072] (2) The solution method is to perform a second-order Taylor expansion of Q t (x t , u t ) at (x t , u t ):

[0073]

[0074] Where Q t (x t + δx, u t + δu) is the second-order Taylor value of the action function of the tth CILQR planning point; δx is the state quantity change corresponding to each time step; δu is the control quantity change corresponding to each time step; T is the transpose; is the first-order partial derivative of Q t (x t , u t ) with respect to x; is the first-order partial derivative of Q t (x t , u t ) with respect to u; is the second-order partial derivative of Q t (x t , u t ) with respect to x; is the second-order partial derivative of Q t (x​​​​​​​t ,u t ) the second-order partial derivative of x and u; Q t (x t ,u t ) the second-order partial derivative of u and x; Q t (x t ,u t ) the second-order partial derivative of u.

[0075] Let the remainder term The problem is converted to finding the minimum of

[0076] (3) Let the derivative be 0, that is, we can get:

[0077]

[0078] When calculating the inverse matrix, a regularization coefficient λ is added to improve the stability of the algorithm.

[0079] According to the relationship between the value function V and the action function Q in (1):

[0080] Q t (x t ,u t ) = L(x t ,u t ) + V t+1 (x t+1 ) = L(x t ,u t ) + V t+1 (f(x t ,u t ))

[0081] Solve respectively:

[0082]

[0083] where L x is the first-order partial derivative of L(x t ,u t ) with respect to x; f x is the first-order partial derivative of the state transition equation f(x t ,u t ) with respect to x; is the first-order partial derivative of V t+1 (x t+1 ) with respect to x; L u is the first-order partial derivative of L(x t ,u t ) with respect to u; f​u The state transition equation is f(x) t ,u t The first-order partial derivative with respect to u; L xx For L(x) t ,u t The second-order partial derivative with respect to x; For V t+1 (x t+1 The second-order partial derivative with respect to x; L ux For L(x) t ,u t Second-order partial derivatives with respect to u and x; L uu For L(x) t ,u t The second-order partial derivative of ) with respect to u.

[0084] Substitute the result The δu that minimizes the objective function can then be calculated, and the latest control quantity at the t-th CILQR planning point can be obtained. Updated to

[0085] Step 223: Based on the initial values ​​of the state variables of the vehicle to be planned and the initial values ​​of the control variables of the starting point under the current iteration number, use forward propagation to determine the state variables corresponding to each CILQR planning point in the trajectory under the current iteration number.

[0086] Specifically, (1) solve for the latest control quantity at the t-th CILQR planning point.

[0087] Then, the state variables of the (t+1)th CILQR planning point can be derived.

[0088]

[0089] At t=0, the pose is the vehicle's pose at the starting point. This application sets five state variables as the vehicle's abscissa, ordinate, heading angle, velocity, and acceleration; and two control variables as the rate of change of acceleration and yaw rate. It is easy to see that the above state-space equations are nonlinear, therefore, the equations need to be linearized.

[0090] (2) Linearization is the process of converting x... t+1 =f(x) t ,u t Transform into x t+1 =Ax t +Bu t Based on the five state variables and two control variables mentioned above, where Δt is the time interval between each planning point, θ is the heading angle, and a is the acceleration, we can derive the 5×5 matrix A and the 5×2 matrix B as follows:

[0091]

[0092] Step 224: using the total cost function, calculating the total cost of the trajectory at the current iteration number based on the state quantity and control quantity corresponding to each CILQR planning point in the trajectory at the current iteration number and the state quantity and control quantity corresponding to each point in the reference line; the total cost function is a function of the control quantity and the state quantity.

[0093] Step 225: based on the total cost of the trajectory at the current iteration number and the total cost of the trajectory at the last iteration number, calculating the cost difference at the current iteration number.

[0094] Step 226: determining whether the stop condition is met; the stop condition is that the cost difference at the current iteration number is less than a preset difference value or a preset iteration number is reached.

[0095] Step 227: if yes, determining the trajectory at the current iteration number as the target trajectory of the vehicle to be planned.

[0096] Step 228: if no, updating the current iteration number to the next iteration number and returning to step 222 until the stop condition is met, and obtaining the target trajectory of the vehicle to be planned.

[0097] As an optional implementation, the total cost function includes: an obstacle distance cost function, a reference line distance cost function, a convex space boundary distance cost function and a control quantity cost function.

[0098] As an optional implementation, the obstacle distance cost function is:

[0099] J obs =q1×exp(q2(X-X0 T P(X-X0))。

[0100] wherein, J obs is the obstacle distance cost function value; q1 and q2 are weight coefficients of the obstacle distance cost function; X is a matrix composed of state quantities corresponding to each CILQR planning point in the trajectory; X0 is a matrix composed of state quantities corresponding to each point in the obstacle; T is a transpose; and P is a matrix containing information of the major and minor axes of the ellipse.

[0101] As an optional implementation, the reference line distance cost function is:

[0102] J ref =(X-X ref ) T p1(X-X ref )。

[0103] wherein, Jref J is the reference line distance cost function value; x is the state variable of each point in the reference line; p1 is the weight coefficient of the reference line distance cost function. ref boundary boundary

[0104] As an optional implementation, the convex space boundary distance cost function is:

[0105] J boundary boundary

[0106] J boundary boundary

[0107] As an optional implementation, the control quantity cost function is:

[0108] J control max min

[0109] J control max min

[0110] Specifically, the formula of the total cost is:

[0111] J all obs ref boundary control T ref T ref boundary max min Specific embodiments​​​​​​​​​​​​​​​​​​​​​

[0113] (1) Establish an obstacle avoidance convex space. Establish a three-lane model, place an obstacle in front of the ego vehicle at a certain position, as shown in Figure 4 and Figure 5 , the three lanes from top to bottom are left lane, current lane and right lane respectively, find the projection points of the four corner points of the obstacle on each lane line, and judge which lane the obstacle is located in. According to the lane where the obstacle is located, the lane changing intention of the ego vehicle can be judged, as shown in Figure 4 In the scenario shown, the ego vehicle intends to change lanes to the right, and the reference line is the right lane center line, and the convex space boundary is the right lane line boundary of the obstacle. Figure 5 In the scenario shown, the ego vehicle intends to go straight, and the reference line is the current lane center line, and the convex space boundary is the left and right boundaries of the current lane.

[0114] (2) Initialize state quantity and control quantity matrix. Because there are five state quantities, x-coordinate, y-coordinate, heading angle, speed and acceleration, the number of CILQR planning points is 20, so the state quantity matrix dimension is 5x20, and two control quantities, acceleration rate and yaw rate, so the control quantity matrix dimension is 2x20. The zero time state quantity is the initial state quantity of the ego vehicle, that is, the first column of the state quantity matrix is [x0, y0, θ0, v0, a0] T The control quantity matrix is 0, so the state quantity and control quantity sequence before the first iteration can be derived according to the state space equation.

[0115] (3) Each iteration in turn carries out the back propagation and forward propagation links to obtain the latest state quantity and control quantity matrix, and calculates the total cost, that is, the sum of the state quantity cost and the control quantity cost J new , compared with the cost J old of the last round, until |J new -J old |<tol, exit the iteration and output the final result. Set the convergence condition tol to 12 and the maximum iteration number to 10. If J new >J old in a certain iteration, do not count the iteration number, change the regularization coefficient λ of the back propagation link, and recalculate the cost.

[0116] (4) Observe the trajectory points generated by CILQR, adjust the weight coefficients appropriately, and generate a smooth trajectory that does not collide with the obstacle and the convex space boundary, as shown in Figure 6 . The planning effect of CILQR can be evaluated by observing the changes of the heading angle and yaw rate of each point.

[0117] The beneficial effects of the present application are:

[0118] (1) The application proposes a method for constructing a convex space, which facilitates the CILQR algorithm to solve in the range of the convex space, prevents different frames from appearing different decision results, and ensures the uniqueness of the solution.

[0119] (2) According to the different positions of the obstacles, a rule-based lane change decision method is proposed, and in order to optimize the time complexity, the binary search method is used to search the projection point of the obstacle on the lane line.

[0120] (3) The application proposes a transverse and longitudinal coupling obstacle avoidance trajectory planning method, which is planned in space and time, can achieve good planning effect in dynamic scene, and improves the safety of obstacle avoidance trajectory planning.

[0121] (4) On the basis of ILQR, the application adds constraint conditions, converts the constraint conditions into optimization objectives, converts the hard constraint into soft constraint, and solves the problem of nonlinear problem difficult to solve by traditional quadratic programming solver through second-order Taylor expansion.

[0122] In an exemplary embodiment, a computer device is provided, comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, the processor executing the computer program to implement the constraint iteration-based LQR motion planning method in embodiment 1.

[0123] In an exemplary embodiment, a computer readable storage medium is provided, having a computer program stored thereon, which is executed by a processor to implement the constraint iteration-based LQR motion planning method in embodiment 1.

[0124] In an exemplary embodiment, a computer program product is provided, comprising a computer program, which is executed by a processor to implement the constraint iteration-based LQR motion planning method in embodiment 1.

[0125] In an exemplary embodiment, a computer device is provided, which can be a server or a terminal, and its internal structure diagram can be as follows Figure 7As shown in the figure. 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 the 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 capability. 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 video tag processing data. The input / output interface of the computer device is used to exchange information between the processor and external devices. The communication interface of the computer device is used to communicate with external terminals through network connection. The computer program is executed by the processor to implement a video tag processing method.

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

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

[0128] 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. The computer program can be stored in a non-volatile computer readable storage medium, and when the computer program is executed, the processes of the above-mentioned embodiments of the methods can be included. Any reference to memory, database or other medium used in the embodiments provided in the present 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 storage, high-density embedded non-volatile memory, resistive memory (ReRAM), magnetoresistive random access memory (MRAM), ferroelectric memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory can include random access memory (RAM) or external cache memory, etc. As an illustration but not limitation, RAM can be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM), etc.

[0129] The database involved in the embodiments provided in the present application can include at least one of a relational database and a non-relational database. The non-relational database can include a distributed database based on a blockchain, etc., without being limited thereto. The processor involved in the embodiments provided in the present application can be a general-purpose processor, a central processing unit, a graphics processing unit, a digital signal processor, a programmable logic device, a data processing logic device based on quantum computing, etc., without being limited thereto.

[0130] The technical features of the above embodiments can be combined arbitrarily. In order to make the description concise, all possible combinations of the technical features in the above embodiments are not described, however, as long as the combinations of the technical features do not exist contradictory, they should be considered as the scope of the present application.

[0131] The principles and implementation modes of the present application are described by applying specific examples herein, and the above-mentioned embodiments are only used to help understand the method and its core idea of the present application; meanwhile, for those skilled in the art, according to the idea of the present application, the specific implementation mode and application range can be changed. In conclusion, the content of the present application should not be understood as a limitation.

Claims

1. A constraint-based iterative LQR motion planning method, characterized in that, The LQR motion planning method based on constraint iteration comprises: constructing a convex space boundary and a reference line according to positions of obstacles in a lane; planning a trajectory for a vehicle to be planned within the convex space boundary based on the reference line; the process of the trajectory planning comprises: obtaining a number of CILQR planning points and initial values of state quantities and control quantities of the vehicle to be planned; the state quantities comprise: a horizontal coordinate, a vertical coordinate, a heading angle, a speed and an acceleration; the control quantities comprise: an acceleration change rate and a yaw rate; updating and iterating the state quantities and the control quantities corresponding to each CILQR planning point by using back propagation and forward propagation based on the initial values of the state quantities and the control quantities of the vehicle to be planned, to obtain a target trajectory of the vehicle to be planned, and complete trajectory planning; updating and iterating the state quantities and the control quantities corresponding to each CILQR planning point by using back propagation and forward propagation based on the initial values of the state quantities and the control quantities of the vehicle to be planned, to obtain a target trajectory of the vehicle to be planned, and complete trajectory planning, comprising: determining any iteration number greater than 1 as a current iteration number; determining the control quantities corresponding to each CILQR planning point in the trajectory at the current iteration number; the control quantities corresponding to each CILQR planning point in the trajectory at the current iteration number are determined by using back propagation based on the control quantities corresponding to each CILQR planning point in the trajectory at a previous iteration number; the control quantities corresponding to each CILQR planning point in the trajectory at an initial iteration number are determined by initialization; determining the state quantities corresponding to each CILQR planning point in the trajectory at the current iteration number by using forward propagation according to the initial values of the state quantities of the vehicle to be planned and initial values of the control quantities of a starting point at the current iteration number; calculating a total cost of the trajectory at the current iteration number based on the state quantities and the control quantities corresponding to each CILQR planning point in the trajectory at the current iteration number and state quantities and control quantities corresponding to each point in the reference line by using a total cost function; the total cost function is a function of the control quantities and the state quantities; calculating a cost difference at the current iteration number based on the total cost of the trajectory at the current iteration number and the total cost of the trajectory at the previous iteration number; determining whether a stop condition is met; the stop condition is that the cost difference at the current iteration number is less than a preset difference value or a preset iteration number is reached; if yes, determining the trajectory at the current iteration number as the target trajectory of the vehicle to be planned; if no, updating the current iteration number as a next iteration number, and returning to "determining the control quantities corresponding to each CILQR planning point in the trajectory at the current iteration number", until the stop condition is met, to obtain the target trajectory of the vehicle to be planned.

2. The constraint-iteration-based LQR motion planning method of claim 1, wherein, The total cost function comprises: an obstacle distance cost function, a reference line distance cost function, a convex space boundary distance cost function and a control quantity cost function.

3. The constraint-iteration-based LQR motion planning method of claim 2, wherein, The obstacle distance cost function is: J obs = q1 x exp(q2(X-X0) T P(X-X0)). wherein J obs is the obstacle distance cost function value; q1 and q2 are both weight coefficients of the obstacle distance cost function; X is a matrix composed of state quantities corresponding to each CILQR planning point in the trajectory; X0 is a matrix composed of state quantities corresponding to each point in the obstacle; T is a transpose; and P is a matrix containing information of the major and minor axes of the ellipse.

4. The constraint-iteration-based LQR motion planning method of claim 3, wherein, The reference line distance cost function is: J ref = (X - X ref ) T p1(X - X ref ); wherein J ref is the reference line distance cost function value; x ref is a matrix composed of state quantities corresponding to each point in the reference line; pi is a weight coefficient of the reference line distance cost function.

5. The constraint-iteration-based LQR motion planning method of claim 4, wherein, The convex space boundary distance cost function is: J boundary = q3 * exp(q4 * (X - X boundary )); wherein J boundary is the convex space boundary distance cost function value; q3 and q4 are both weight coefficients of the convex space boundary distance cost function; X boundary is a matrix composed of state quantities corresponding to the projection points of each CILQR planning point in the trajectory on the convex space boundary.

6. The constraint-iteration-based LQR motion planning method of claim 5, wherein, The control quantity cost function is: J control = q5 * exp(q6(U - U max ))+ q7 * exp(q8(U min -U)) ; wherein J control is the control cost function value; q5, q6 and q7 are weight coefficients of the control cost function; U is a matrix composed of control quantities corresponding to each CILQR planning point in the trajectory; U max is the maximum value in the matrix composed of control quantities corresponding to each CILQR planning point in the trajectory; U min is the minimum value in the matrix composed of control quantities corresponding to each CILQR planning point in the trajectory.

7. A computer apparatus comprising: A memory, a processor, and a computer program stored on the memory and executable on the processor, characterized in that the processor executes the computer program to implement the constraint-iteration-based LQR motion planning method of any one of claims 1-6.

8. A computer readable storage medium having stored thereon a computer program, characterized in that, The computer program is executed by the processor to implement the constraint-iteration-based LQR motion planning method of any one of claims 1-6.

9. A computer program product comprising a computer program, characterized in that, The computer program is executed by the processor to implement the constraint-iteration-based LQR motion planning method of any one of claims 1-6.

Images