Vehicle trajectory planning method and device, unmanned vehicle and storage medium

By using linearized action value function and obstacle function sub-item recursive optimal control rate under the Frenet coordinate system, the problem of horizontal trajectory planning in vehicle trajectory planning is solved, and efficient trajectory correction and planning is achieved to meet road boundaries and curvature constraint.

CN118915756APending Publication Date: 2024-11-08NEOLITHIC HUITONG TECHNOLOGY CO LTD
View PDF 0 Cites 2 Cited by

Patent Information

Application Number
CN202411018224.5
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-07-26
Publication Date
2024-11-08

AI Technical Summary

Technical Problem

Under the Frenet coordinate system, it is difficult for the prior art to effectively realize the lateral trajectory planning of vehicles, especially trajectory corrections under the conditions of road boundary and curvature constraints.

Method used

By obtaining the driving trajectory and road boundary information of the target vehicle, using a linear action value function, the optimal control rate of each step is redirected from the end point of the driving trajectory to the initial point, correct the trajectory, and construct the action value based on the obstacle function sub-item Functions, including corner point constraints and curvature constraints, repeat recurrence until the preset goal is reached.

Benefits of technology

The vehicle trajectory planning problem is simplified, the solution efficiency is improved, and the trajectory meets road boundary and curvature constraints under the Frenet coordinate system, achieving more efficient trajectory correction and planning.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118915756A_ABST
    Figure CN118915756A_ABST
Patent Text Reader

Abstract

The invention discloses a vehicle trajectory planning method and device, an unmanned vehicle and a storage medium. The method comprises the following steps: acquiring a driving trajectory and obstacle information of a target vehicle; based on the driving track and a linearized action value function, the optimal control rate of each step is recursively calculated from the end point of the driving track to the initial point so as to correct the driving track, the action value function comprises a barrier function subitem, and the barrier function subitem is constructed based on the angular point constraint of the road boundary information on the target vehicle and the curvature constraint of the driving track; and based on the corrected driving track, repeatedly recurring the optimal control rate of each step until a preset target is reached so as to complete track planning of the target vehicle. According to the track planning mode, the transverse track planning requirement under the frenet coordinate system can be met, the global optimization problem is converted into the independent local optimization problem in each step, the vehicle track planning problem is simplified, and the solving efficiency is higher.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present application belongs to the field of autonomous driving technology, and specifically relates to a vehicle trajectory planning method and device, an unmanned vehicle, and a storage medium. Background Art

[0002] Trajectory planning algorithm refers to calculating the trajectory of an object or vehicle in space under given time and space constraints to complete a specific task or achieve a specific goal. This algorithm is generally widely used in fields such as autonomous driving, aircraft path planning, and robot motion control. The goal of the trajectory planning algorithm is to find a continuous trajectory so that the object or vehicle can reach the target position within a specified time while satisfying various constraints, such as avoiding obstacles, maintaining a safe distance, and complying with dynamic restrictions. Trajectory planning can be decoupled into lateral and longitudinal planning. Lateral planning can be regarded as planning in the driving direction, which determines the shape of the trajectory. How to implement lateral trajectory planning in the Frenet coordinate system is an urgent problem to be solved.

[0003] The information disclosed in this background technology section is only intended to increase the understanding of the overall background of the application and should not be regarded as an admission or any form of suggestion that the information constitutes the prior art already known to a person skilled in the art. Summary of the invention

[0004] The purpose of this application is to provide a vehicle trajectory planning method, which is used to solve the problem of how to implement lateral trajectory planning in a frenet coordinate system.

[0005] To achieve the above objectives, the present application provides a vehicle trajectory planning method, the method comprising:

[0006] Obtain the target vehicle's driving trajectory and road boundary information;

[0007] Based on the driving trajectory and the linearized action value function, recursively deducing the optimal control rate of each step from the end point of the driving trajectory to the initial point to correct the driving trajectory, wherein the action value function includes an obstacle function sub-item, and the obstacle function sub-item is constructed based on the corner point constraint of the target vehicle and the curvature constraint of the driving trajectory based on the road boundary information;

[0008] Based on the corrected driving trajectory, the optimal control rate of each step is repeatedly recursively deduced and the driving trajectory is corrected to complete the trajectory planning of the target vehicle.

[0009] In one embodiment, the method for constructing the barrier function sub-item includes:

[0010] Determine a first reference circle enveloping a front corner point of the target vehicle, and a second reference circle enveloping a rear corner point of the target vehicle, wherein the centers of the first reference circle and the second reference circle are located on the axle line of the target vehicle;

[0011] Based on the collision constraints of the road boundary information on the first reference circle and the second reference circle, a corner point constraint on the target vehicle is constructed.

[0012] In one embodiment, the vehicle trajectory planning method according to claim 1 is characterized in that the first reference circle passes through the front corner point of the target vehicle; and / or the second reference circle passes through the rear corner point of the target vehicle; and / or,

[0013] Constructing a corner point constraint for the target vehicle based on the collision constraint of the first reference circle and the second reference circle by the road boundary information specifically includes:

[0014] The center position of the first reference circle is constrained to a minimum value by the sum of the radius of the lower boundary of the road and the first reference circle, and the center position of the first reference circle is constrained to a maximum value by the difference between the radius of the upper boundary of the road and the first reference circle; and / or,

[0015] The center position of the second reference circle is constrained to a minimum value by the sum of the lower boundary of the road and the radius of the second reference circle, and the center position of the second reference circle is constrained to a maximum value by the difference between the upper boundary of the road and the radius of the second reference circle.

[0016] In one embodiment, the method for constructing the barrier function sub-item includes:

[0017] Determining a curvature feedforward of the target vehicle at a current step based on the driving trajectory of the target vehicle;

[0018] The maximum turning angle of the target vehicle is used to constrain the sum of the curvature of the driving trajectory of the target vehicle at the current step and the curvature feedforward.

[0019] In one embodiment, the barrier function sub-item is any one of an exponential function, a logarithmic function, and a square function.

[0020] In one embodiment, based on the driving trajectory and the linearized action value function, the optimal control rate of each step is recursively deduced from the end point of the current driving trajectory to the initial point, specifically including:

[0021] Based on the state quantity of the driving trajectory, determining the first-order gradient vector and the Hessian matrix of the cost function corresponding to the trajectory point, and the Jacobian matrix of the system state equation;

[0022] Determine the optimal control rate of the Tth step based on the first-order gradient vector and the Hessian matrix of the cost function of the corresponding trajectory point, the Jacobian matrix of the system state equation, and the linearized action value function;

[0023] Based on the optimal control rate of the Tth step and the linearized action value function, the state value correction amount of the Tth step is determined, and then the optimal control rate of the T-1th step is recursively deduced.

[0024] In one embodiment, the method further comprises:

[0025] Performing a second-order Taylor expansion on the cost function and converting it into a model of trajectory point cost error;

[0026] The system state equation is subjected to first-order Taylor expansion and converted into a model of trajectory point state quantity error.

[0027] In one embodiment, the optimal control rate includes a control amount correction amount of each trajectory point of the driving trajectory;

[0028] The method further comprises:

[0029] Based on the control amount correction amount, the control amount of each trajectory point of the driving trajectory is corrected, thereby correcting the driving trajectory.

[0030] The present application also provides a vehicle trajectory planning device, comprising:

[0031] An acquisition module is used to obtain the driving trajectory and obstacle information of the target vehicle;

[0032] A correction module, configured to recursively deduce the optimal control rate of each step from the end point of the driving trajectory to the initial point based on the driving trajectory and a linearized action value function, so as to correct the driving trajectory, wherein the action value function includes an obstacle function sub-item, and the obstacle function sub-item is constructed based on the corner point constraint of the target vehicle and the curvature constraint of the driving trajectory based on the road boundary information;

[0033] The planning module is used to repeatedly recursively deduce the optimal control rate of each step based on the corrected driving trajectory until a preset target is reached to complete the trajectory planning of the target vehicle.

[0034] The present application also provides an unmanned vehicle, comprising:

[0035] at least one processor; and

[0036] A memory storing instructions, which, when executed by the at least one processor, enables the at least one processor to execute the vehicle trajectory planning method as described above.

[0037] The present application also provides a machine-readable storage medium storing executable instructions, which, when executed, enable the machine to execute the vehicle trajectory planning method as described above.

[0038] Compared with the prior art, according to the vehicle trajectory planning method of the present application, the optimal control rate of each step is recursively deduced from the end point of the driving trajectory to the initial point through a linearized action value function to correct the driving trajectory; at the same time, the above-mentioned steps of recursively deducing the optimal control rate of each step are continuously repeated based on the corrected driving trajectory until the preset target is reached; in this process, the action value function includes the obstacle function sub-item constructed based on the corner point constraint of the target vehicle and the curvature constraint of the driving trajectory based on the road boundary information, which can meet the needs of lateral trajectory planning in the Frenet coordinate system. Since the global optimization problem is converted into an independent local optimization problem at each step, the vehicle trajectory planning problem is simplified and the solution efficiency is higher.

[0039] On the other hand, during the solution process, only a first-order Taylor expansion is performed on the system state equation. Since only the Jacobian matrix needs to be calculated, the time required for iteration is much less than the time required for the second-order Taylor expansion to calculate the Hessian matrix, further improving the solution efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] Figure 1 This is an application scenario diagram of a vehicle trajectory planning method according to an embodiment of the present application;

[0041] Figure 2 is a flow chart of a vehicle trajectory planning method according to an embodiment of the present application;

[0042] Figure 3 and Figure 4 A scene graph in which a front corner point and a rear corner point of a target vehicle are enclosed by a first reference circle and a second reference circle in a vehicle trajectory planning method according to different embodiments of the present application;

[0043] Figure 5 A module diagram of a vehicle trajectory planning device according to an embodiment of the present application;

[0044] Figure 6 It is a hardware structure diagram of an unmanned vehicle according to an embodiment of the present application. DETAILED DESCRIPTION

[0045] The present application will be described in detail below in conjunction with the various embodiments shown in the accompanying drawings. However, these embodiments do not limit the present application, and any structural, methodological, or functional changes made by a person of ordinary skill in the art based on these embodiments are all within the scope of protection of the present application.

[0046] The terms "first", "second", "third", "fourth", etc. (if any) in the specification and claims of the present application and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that the data used in this way can be interchangeable where appropriate, so that the embodiments of the present application described herein can be implemented in an order other than those illustrated or described herein, for example. In addition, the terms "including" and "corresponding to" and any of their variations are intended to cover non-exclusive inclusions, for example, a process, method, system, product or device that includes a series of steps or units is not necessarily limited to those steps or units that are clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0047] Before introducing the embodiments of the present application, the basic technologies and some technical terms involved in the embodiments of the present application are schematically explained:

[0048] Autonomous driving system: A system that implements different levels of autonomous driving functions of a vehicle, such as assisted driving system (L2), high-speed autonomous driving system that requires human supervision (L3), and highly / fully autonomous driving system (L4 / L5).

[0049] Intelligent Traffic System (ITS), also known as Intelligent Transportation System, effectively and comprehensively applies advanced science and technology (information technology, computer technology, data communication technology, sensor technology, electronic control technology, automatic control theory, operations research and artificial intelligence, etc.) to transportation, service control and vehicle manufacturing, strengthens the connection between vehicles, roads and users, and thus forms a comprehensive transportation system that ensures safety, improves efficiency, improves the environment and saves energy.

[0050] Intelligent Vehicle Infrastructure Cooperative Systems (IVICS), referred to as IVICS, is a development direction of intelligent transportation systems. IVICS uses advanced wireless communications and new generation Internet technologies to implement dynamic real-time information interaction between vehicles and between vehicles and roads in all directions, and conducts active vehicle safety control and road cooperative management based on the collection and integration of dynamic traffic information in all time and space, fully realizing the effective coordination of people, vehicles and roads, ensuring traffic safety, and improving traffic efficiency, thus forming a safe, efficient and environmentally friendly road traffic system.

[0051] Trajectory planning refers to the initial state of a given vehicle (including starting position, speed and acceleration), target state (including target position, speed and acceleration), obstacle position, as well as dynamics and comfort. A smooth trajectory is calculated based on the constraints, and the vehicle is guided along this trajectory to the target state. Trajectory planning consists of two parts: path planning and speed planning. Path planning is responsible for calculating a smooth path from the starting position to the target position, while speed planning calculates the speed of each path point based on this path to form a speed curve.

[0052] Frenet coordinate system: also called road coordinate system, with the vehicle's starting position as the origin. The coordinate axes are perpendicular to each other and are divided into the S-axis direction (i.e. the tangent direction along the road reference line, called the longitudinal direction) and the L-axis direction (i.e. the current normal direction of the reference line, called the lateral direction). The coordinates are expressed as (S, L).

[0053] Figure 1 FIG. 1 is an optional schematic diagram of the system architecture involved in the embodiment of the present application. Figure 1 As shown, the sensor (such as laser radar, visual sensor, etc.) collects the boundary information of the road on which the target vehicle is currently traveling. The sensor sends the collected road boundary information to the server. After receiving the road boundary information sent by the sensor, the server plans the driving trajectory. At this time, the planned driving trajectory may interfere with the road boundary, or the curvature exceeds a certain limit. The server then uses the driving trajectory as the initial trajectory, and combines the linearized action value function to recursively deduce the optimal control rate of each step from the end point of the driving trajectory to the initial point to correct the driving trajectory. The server also repeatedly recursively deduce the optimal control rate of each step with the corrected driving trajectory and corrects the driving trajectory again, thereby completing the trajectory planning of the target vehicle.

[0054] Continue to participate Figure 1, in a specific scenario of applying the vehicle trajectory planning method of the present application, a server and a terminal device may be included. Among them, the terminal device may include a vehicle-mounted terminal and a user terminal. The vehicle-mounted terminal may include a driving computer or an on-board unit (On Board Unit, OBU), etc. The vehicle-mounted terminal may also be an application (application, APP) on the terminal, an APP on the smart rearview mirror, an APP or a small program on a mobile phone, etc., which are not limited here. The user terminal (user equipment, UE) can be a wireless terminal device or a wired terminal device. The wireless terminal device may refer to a device with a wireless transceiver function. The user terminal may be a mobile phone (mobile phone), a tablet computer (Pad), a computer with a wireless transceiver function, a virtual reality (Virtual Reality, VR) user device, an augmented reality (Augmented Reality, AR) user device, an intelligent voice interaction device, a smart home appliance, a vehicle-mounted terminal, an aircraft, etc., which are not limited here.

[0055] The terminal communicates with the server through the network. The data storage system can store the data that the server needs to process. The data storage system can exist independently or be integrated on the server. The server obtains the driving trajectory and road boundary information of the target vehicle, and based on the driving trajectory and the linearized action value function, recursively deduce the optimal control rate of each step from the end point of the driving trajectory to the initial point to correct the driving trajectory; then, based on the corrected driving trajectory, repeatedly recursively deduce the optimal control rate of each step and correct the driving trajectory to complete the trajectory planning of the target vehicle.

[0056] It can be understood that the above is just an example of a possible execution subject of the vehicle trajectory planning method of the present application, which is explained by way of example using a server. In more embodiments, the vehicle trajectory planning can also be directly executed by a terminal device with sufficient computing power, and the present application does not limit this. Moreover, regardless of the type of server / terminal device, the vehicle trajectory planning method provided in the embodiment of the present application can be adapted to the autonomous driving system of unmanned vehicles, including L2, L3, L4 and above autonomous driving systems.

[0057] Specific reference Figure 2 , introduces an embodiment of the vehicle trajectory planning method of the present application. In this embodiment, the method includes:

[0058] S11. Obtain the driving trajectory and road boundary information of the target vehicle.

[0059] The driving trajectory of the target vehicle may be stored on a server, and the target vehicle may be any vehicle traveling in the target area, and the corresponding driving trajectory is obtained by initiating a driving trajectory acquisition request of the target vehicle to the server. Alternatively, the target vehicle may be several or all vehicles specified in the target area, and the corresponding driving trajectory is obtained by initiating a driving trajectory acquisition request of these part / all vehicles to the server. The driving trajectory of the target vehicle may also be stored on the corresponding unmanned vehicle, and the corresponding driving trajectory is obtained by initiating a driving trajectory acquisition request to the unmanned vehicle, and this application does not impose any restrictions on this.

[0060] In the embodiments of the present application, the road boundary may include a double yellow line for distinguishing the direction of vehicle travel, the actual physical boundary of the road, or any form of boundary line of the lane where the vehicle is located, etc., and the present application does not limit this. At the same time, the road boundary is defined based on the Frenet coordinate system, and the boundary with a larger coordinate in the L-axis direction (lateral) of the road is called the upper boundary, and the boundary with a smaller coordinate in the L-axis direction is called the lower boundary. The road boundary information may include the above-mentioned upper boundary information and lower boundary information, specifically, for example, the upper boundary L-axis direction coordinate and the lower boundary L-axis direction coordinate corresponding to the current position of the target vehicle.

[0061] S12. Based on the driving trajectory and the linearized action value function, the optimal control rate of each step is recursively deduced from the end point of the driving trajectory to the initial point to correct the driving trajectory.

[0062] S13. Based on the corrected driving trajectory, repeatedly recursively deduce the optimal control rate of each step and correct the driving trajectory to complete the trajectory planning of the target vehicle.

[0063] The vehicle's driving trajectory can be understood as the process of transferring the vehicle state from the current state to the next state given a step-length control amount in a discrete time series. The entire trajectory planning process is to find an optimal control sequence from the current state to transfer the vehicle from the current state to the target state after a specified number of time steps.

[0064] Furthermore, based on the idea of ​​dynamic programming, the following are defined in the embodiments of the present application:

[0065] State value V(k): the minimum cumulative cost from any point k on the driving trajectory to the end point, defined as the state value of the point;

[0066] A control quantity is given from any point k on the driving trajectory, so that the cost of transferring from node k to node k+1 according to the system state equation is q(k);

[0067] Action value Q(k): Given an arbitrary control amount from any point k on the driving trajectory so that the target vehicle is transferred to the k+1 node, and the optimal control amount can always be selected from the k+1 node to the end point, then it is determined that any point k has an action value Q(k)=q(k)+V(k+1) under this control amount.

[0068] The above V(k), q(k) and Q(k) are all scalars representing costs. Based on the above definitions, the problem of finding the optimal control sequence u1, u2, u3, …, un can be converted into a nested sub-problem of finding the optimal control quantity from the terminal state of the driving trajectory.

[0069] In order to better illustrate the recursive process of the optimal control rate in this embodiment, it is assumed that the cost function and state transfer equation in the trajectory planning process are linear. The cost function to be optimized is written as a quadratic function of the state quantity X and the control quantity U, and the kinematic equation constraint is written as a linear combination of the state quantity X and the control quantity U, as shown below:

[0070]

[0071] Among them, C(k) is the cost function, F is the system state equation, f is the feedforward, C ux =C xu T , the C matrix and D matrix in C(k) can be set according to the actual control requirements. The subscripts in the C matrix and D matrix indicate that they will be multiplied with the corresponding X and U after expansion.

[0072] Exemplarily, the state quantity X may include S and L values, vehicle speed, and heading angle in the Frenet coordinate system, and the control quantity U may include acceleration, heading angle change rate, and the like.

[0073] Based on the above settings, the optimal control rate and the corresponding state value matrix C(k-1) of the previous point (previous step) can be obtained from the terminal state X(n) of the driving trajectory, and then gradually recursively forward to the initial point state X(1) of the driving trajectory. Subsequently, the optimal control amount U and the optimal state X of each step are solved forward to the terminal point according to the optimal control rate K(1) of the corresponding step of the initial point.

[0074] For example, assuming that the driving trajectory has 100 sampling points, since the end point is the target state, it can be considered that V(100)=0 (no cost).

[0075] Recursively push forward one step from the end point, and at step 99, the action value can be determined:

[0076]

[0077] If we want to solve the optimal control quantity of step 99 so that Q(99) reaches the minimum value, we can take the partial derivative of Q(99) with respect to the control quantity U, and then determine the extreme point of the action value of step 99. Let:

[0078]

[0079] The optimal control rate at step 99 can be obtained:

[0080]

[0081] Substitute the optimal control rate of step 99 into the action value function Q(99) of step 99 to eliminate U 99 , get the state value of step 99:

[0082]

[0083] Among them, the added constant const can make it more universal.

[0084] So far, the action value, state value, and K and k matrices of the 99th step have been obtained.

[0085] Starting from step 99, we move forward one step again and calculate the action value at step 98:

[0086]

[0087] And because:

[0088]

[0089] Therefore, the above formula can be expanded to:

[0090]

[0091] Transforming again, Q(98) can be written as:

[0092]

[0093] It can be seen that it has become a quadratic form again. Then, the quadratic form is used to find the partial derivative of the control quantity U to solve the optimal control law of step 98:

[0094]

[0095] Similarly, substituting the optimal control law of step 98 into the action value function Q(98) of step 98 again yields the state value of step 98:

[0096]

[0097] Based on the above method, we continue to recursively move forward until we reach the initial point of the driving trajectory (the first step). Thus, we can obtain the optimal control rate for each step on the driving trajectory. Since the state of the initial point of the driving trajectory is known, we interact the obtained optimal control rate with the state of the initial point to obtain the control amount of the first step, and then obtain the state amount of the first step. Repeat this process to complete a round of correction of the initial driving trajectory. The process is shown as follows:

[0098] u t =K t x t +k t

[0099] x t+1 =F(x t ,u t )

[0100] Among them, u t is the control quantity of step t, x t and x t+1 is the state quantity of the tth and t+1th steps, and F is the system state equation.

[0101] In the above derivation process, the premise of the derivation is to assume that the cost function and state transition equation in the trajectory planning process are linear. However, in this type of planning problem, the cost function may include nonlinear terms, and the system state equation is usually nonlinear. This type of nonlinear model is expressed as follows:

[0102] X k+1 =f(X k ,U k ) (4)

[0103] C k+1 = l(X k ,U k ) (5)

[0104] In this embodiment, the above nonlinear model can be linearized using Taylor expansion. In particular, the system state equation f can be expanded by a first order Taylor, and the cost function l can be expanded by a second order Taylor. This linearization method comprehensively considers factors such as the amount of calculation of a single iteration and the number of iterations in the trajectory planning process.

[0105] The system state equation f after the first-order Taylor expansion is expressed as follows:

[0106]

[0107] The cost function l after the second-order Taylor expansion is expressed as follows:

[0108]

[0109] The system state equation and cost function after the above linear expansion are converted into the model of trajectory point state quantity error and the model of cost error, which are expressed as follows:

[0110]

[0111] It can be seen that after the above processing, the model of the trajectory point cost error becomes a quadratic form, and the trajectory point state error becomes a linear combination of input increments (state increments and control increments). Therefore, the solution method for correcting the driving trajectory in the above linear model can be used similarly.

[0112] In this embodiment, the first-order gradient vector and Hessian matrix of the cost function corresponding to the trajectory point and the Jacobian matrix of the system state equation can be determined based on the state quantity of the driving trajectory.

[0113]

[0114] Among them, l k is the first-order gradient vector of the cost function at trajectory point k, l x is the partial derivative of the cost function with respect to the state quantity, l u is the partial derivative of the cost function with respect to the control quantity, l xu and l ux is the second-order mixed partial derivative of the cost function to the state quantity and control quantity, l xx is the second-order partial derivative of the cost function with respect to the state quantity, l uu is the second-order partial derivative of the cost function with respect to the control quantity, L k is the Hessian matrix of the cost function at trajectory point k, F k is the Jacobian matrix of the system state equation at trajectory point k.

[0115] Subsequently, the optimal control rate of the Tth step can be determined based on the first-order gradient vector and Hessian matrix of the cost function of the corresponding trajectory point, the Jacobian matrix of the system state equation, and the linearized action-value function.

[0116] Similarly, based on the above definitions of state value and action value, state value modifiers and action value modifiers are adaptively defined.

[0117] Exemplarily, the state value modifier is expressed as follows:

[0118]

[0119] Action value modifiers are expressed as follows:

[0120]

[0121] The kinematic equality constraint is expressed as follows:

[0122]

[0123] Among them, δX is the state quantity correction, δU is the control quantity correction, f x is the state input matrix, and f u is the control input matrix. δQ(k) can be regarded as a linearized action-value function.

[0124] Substituting equation (15) and equation (13) into equation (14), we can obtain:

[0125]

[0126] Rearranging equation (16) yields:

[0127]

[0128] Q x = l x +f x T v k+1

[0129] Q u = l u +f u T v k+1

[0130] Q xx = l xx +f x T V k+1 f x

[0131] Q xu = l xu +f x T v k+1 f u

[0132] Q ux = l ux +f u T V k+1 f x

[0133] Q uu = l uu +f u T V k+1 f u

[0134] Similarly, by taking the partial derivative of δQ(k) with respect to δU, we can obtain the optimal control rate for the current step (i.e., the Tth step):

[0135]

[0136] Finally, based on the optimal control rate of the Tth step and the linearized action value function, the state value correction of the Tth step is determined, and then the optimal control rate of the T-1th step is recursively deduced.

[0137] Specifically, the optimal control rate of formula (18) can be substituted into the linearized action value function of formula (17) to obtain:

[0138]

[0139] V k =Q xx +K k T Q uu K k +K k T Q ux +Q ux T K k

[0140] v k =Q x +K k T Q uu k k +K k T Q u +Q ux T k k

[0141] At this point, based on the state value correction δV(k) of the Tth step, the action value correction δV(k-1) of the T-1th step can be recursively deduced, and then the optimal control rate of the T-1th step can be obtained.

[0142] It can be seen that in the above process of recursively deriving the optimal control rate of each step from the end point of the driving trajectory to the initial point, the control amount correction amount δU of each trajectory point (each step) of the driving trajectory can be continuously solved. In this embodiment, the control amount of each trajectory point of the driving trajectory can be corrected based on the control amount correction amount, thereby realizing the correction of the driving trajectory. The control amount U correction process of each trajectory point can be expressed as:

[0143]

[0144] X k+1 =f(X k ,Uk ) (twenty one)

[0145] in, is the Taylor expansion point of the driving trajectory.

[0146] The corrected driving trajectory can be regarded as the current rough solution. With the corrected driving trajectory and the linearized action value function, the above recursive optimal control rate and driving trajectory correction process are repeated until the preset target is reached. At this point, the final corrected driving trajectory can be obtained.

[0147] The preset target here can be adjusted according to the actual scenario application requirements. For example, the target rounds of repeated recursion, the specified driving trajectory state quantity convergence, etc. can be set. This application does not impose any restrictions on this.

[0148] In the embodiment of the present application, the trajectory planning of the vehicle mainly considers planning a driving trajectory in the Frenet coordinate system in which the corner points of the vehicle do not interfere with the road boundary and the curvature does not exceed the steering wheel angle constraint of the vehicle. The driving trajectory of the target vehicle is fitted with a multi-segment third-order spline curve. The i+1th segment of the third-order spline curve can be expressed as:

[0149]

[0150] Among them, the domain of the third-order spline curve segment is s i <s<s i+1 , that is, the S-axis coordinate length of span △s in the Frenet coordinate system, we know that s i+1 -s i =Δs.

[0151] Deriving equation (22) with respect to S, we can obtain S i The derivatives of the curve in the S-axis direction are as follows:

[0152]

[0153] Among them, l i (s i ),l' i (s i ),l” i (s i ),l”' i (s i ) represent l i In S i The first, second, and third derivatives at , that is, l i (s i ),l' i (s i ),l” i (s i ),l”'i (s i ) represent the target vehicle in S i The speed, acceleration and impact degree at the location.

[0154] Formula (23) is expressed in matrix form as follows:

[0155]

[0156] Similarly, S i+1 The derivatives of the curve in the S-axis direction are as follows:

[0157]

[0158] Since the third-order spline curve segment is second-order differentiable at each connection point, that is, the curvature is continuous, we can get:

[0159]

[0160] It will be abbreviated as follows: i (s i+1 )=l i+1 (s i+1 )=l i+1 wait.

[0161] Mathematically, if the state quantity of the starting point of a spline curve, i.e., the value of the second-order internal derivative, is known, the analytical expression of the spline curve and the state quantity of the end point of the spline curve can be uniquely determined by obtaining the third-order derivative of the parameter variable at the starting point. Therefore, the above formula (26) can be expressed by a matrix model as follows:

[0162]

[0163] Therefore, based on formula (27), the discrete state transfer equation based on the third-order spline curve in the Frenet coordinate system can be established.

[0164] Furthermore, according to the mapping of the spline curve endpoint state and the spline curve model coefficient, we can obtain:

[0165]

[0166] Keeping the forward Euler difference step size consistent with the spline step size yields:

[0167]

[0168] Formula (29) can be rearranged to obtain:

[0169]

[0170] Assuming the state variable (observable quantity) of the model is X = [l, l', l"], and the control quantity is U = [l"'], we can get:

[0171]

[0172] f1,f2,f3 can be expressed as:

[0173]

[0174] Formula (31) can be changed into the error model:

[0175]

[0176] Based on formula (32), the model is linearized to obtain:

[0177]

[0178] It can be seen that based on formula (33), the optimal control rate of each step can be recursively deduced from the end point of the third-order spline driving trajectory to the initial point, and the control amount correction amount of each trajectory point (each step) is continuously solved. After the driving trajectory is corrected, based on the linearized action value function, the optimal control rate of each step and the correction process of the driving trajectory are repeatedly recursively deduced until the preset target is reached. At this time, the driving trajectory based on the third-order spline curve in the Frenet coordinate system can be obtained.

[0179] In the process of planning the target vehicle's driving trajectory, inequality constraints cannot be directly considered. Therefore, in this embodiment, the reward inequality constraint is converted into an equivalent cost in the form of an obstacle function for processing. By adding an obstacle function sub-item to the action value function, the obstacle function sub-item is constructed based on the road boundary information to constrain the corner points of the target vehicle and the curvature constraint of the driving trajectory. When the linearized action value function is used to correct the driving trajectory, the road boundary information and curvature constraint information are utilized.

[0180] The corner point constraint of the target vehicle imposed by the road boundary information may be, for example, a set safety distance between the designated corner point of the constrained vehicle and the road boundary. The setting of the safety distance may be an empirical value, or may be related to the current vehicle state (such as vehicle speed) of the target vehicle, etc. This application does not impose any restrictions on this.

[0181] Ginseng Figure 3 In one embodiment, a first reference circle enveloping the front corner point of the target vehicle and a second reference circle enveloping the rear corner point of the target vehicle can be determined, and based on the road boundary information, collision constraints on the first reference circle and the second reference circle are constructed to construct corner point constraints on the target vehicle, and the first reference circle and the second reference circle can be determined based on a variety of methods.

[0182] For example, when the first reference circle and the second reference circle envelop the front angle, they have a certain expansion relative to the body of the target vehicle. Assuming that the centers of the first reference circle and the second reference circle are located on the axle line of the target vehicle, the distance between the center of the first reference circle and the front corner of the target vehicle is r1, and the distance between the center of the second reference circle and the rear corner of the target vehicle is r2, r1 corresponds to an expansion coefficient α1 greater than 1, and r2 corresponds to an expansion coefficient α2 greater than 1, then the radius of the first reference circle can be determined to be r1*α1, and the radius of the second reference circle can be determined to be r2*α2. For another example, the above-mentioned expansion coefficients α1 and α2 may not be a fixed value, but are adjusted according to the speed of the target vehicle. The higher the speed, the larger the corresponding expansion coefficients α1 and α2, and the smaller the speed, the smaller the corresponding expansion coefficients α1 and α2.

[0183] Based on the first reference circle enveloping the corner points of the front of the vehicle and the second reference circle enveloping the corner points of the rear of the vehicle, a rough simulation of the position of the vehicle body can be achieved, and then by detecting the collision between the first reference circle and the second reference circle and the road boundary, the corner point constraints of the target vehicle can be realized by utilizing the road boundary information.

[0184] With reference Figure 4 In one embodiment, the first reference circle and the second reference circle are set as follows: the centers of the first reference circle and the second reference circle are located on the axle line of the target vehicle, and the first reference circle passes through the front corner of the target vehicle, and the second reference circle passes through the rear corner of the target vehicle. Based on this setting, it can be obtained:

[0185]

[0186] Wherein, R1 is the radius of the first reference circle, d_f1 is the distance between the center of the first reference circle and the midpoint of the front of the vehicle, R2 is the radius of the second reference circle, d_f2 is the distance between the center of the second reference circle and the midpoint of the rear of the vehicle, and w is the vehicle body width.

[0187] After obtaining the radius of the first reference circle and the radius of the second reference circle, for the first reference circle: the center position of the first reference circle can be constrained to the minimum value by the sum of the radius of the lower boundary of the road and the first reference circle, and the center position of the first reference circle can be constrained to the maximum value by the difference between the upper boundary of the road and the radius of the first reference circle; for the second reference circle: the center position of the second reference circle can be constrained to the minimum value by the sum of the radius of the lower boundary of the road and the second reference circle, and the center position of the second reference circle can be constrained to the maximum value by the difference between the upper boundary of the road and the radius of the second reference circle. The specific expressions are as follows:

[0188] l bound_min +R1 <l+c_f1·sin(arctan(l’))<l bound_max -R1 (36)

[0189] lbound_min +R2 <l-c_f2·sin(arctan(l’))<l bound_max -R2 (37)

[0190] Wherein, c_f1 is the distance between the center of the first reference circle and the center of the rear axle of the target vehicle, c_f2 is the distance between the center of the second reference circle and the center of the front axle of the target vehicle, and l bound_min is the L-axis coordinate of the boundary of the road, l bound_max is the L-axis coordinate of the lower boundary of the road, and l is the L-axis coordinate of the driving trajectory.

[0191] When converting the inequality constraint into the form of a barrier function, it can be constructed based on different forms of barrier functions, such as any one of an exponential function, a logarithmic function, and a square function, and the present application does not impose any restrictions on this.

[0192] Taking the exponential function as an example, we can first convert the constraint condition of formula (36) into an equation as follows:

[0193] d1=l+c_f1·sin(arctan(l'))-l bound_max +R1 (38)

[0194] d2=l bound_min +R1-l-c_f1·sin(arctan(l')) (39)

[0195] The transformed barrier function sub-term can be expressed as:

[0196]

[0197] Among them, q1 and q2 are constants set according to requirements.

[0198] It can be seen that when d1 ≥ 0 or d2 ≥ 0, the value of the barrier function tends to infinity. Therefore, adding the barrier function sub-item to the action value function is equivalent to constraining the barrier function sub-item to tend to the minimum value, thereby satisfying the constraint condition of formula (36).

[0199] Similarly, the constraint condition of formula (37) can also be implemented by constructing the barrier function sub-item as mentioned above, which will not be repeated here.

[0200] The curvature constraint of the driving trajectory may be a maximum value constraint on the curvature of the driving trajectory, so as to ensure the comfort and safety of the trajectory. The maximum value constraint of the curvature may be a fixed value preset based on experience, or the maximum turning angle of the target vehicle, that is, the steering wheel angle obtained by the inverse kinematics of the planned driving trajectory cannot be greater than the maximum steering wheel angle allowed by the target vehicle.

[0201] In one embodiment, the curvature feedforward of the target vehicle at the current step can be determined based on the target vehicle's driving trajectory; and the sum of the curvature of the target vehicle's driving trajectory at the current step and the curvature feedforward can be constrained by the target vehicle's maximum turning angle. The specific expression is as follows:

[0202]

[0203] Among them, R min is the minimum curvature radius corresponding to the maximum turning angle of the target vehicle, △θ is the difference between the heading of the target vehicle and the heading angle of the road, l is the L-axis coordinate of the driving trajectory, k r is the curvature of the road.

[0204] Similarly, equation (41) can be converted into equation form as follows:

[0205]

[0206] Taking equation (43) as an example, we can calculate the result of the previous trajectory planning and q1e q The first and second derivatives of 2d3. Let q1e q 2d3=cost(x,u)=C(u), the second-order Taylor expansion is:

[0207]

[0208] At this point, the linearized obstacle function shown in formula (45) can be used as a sub-item of the state value modifier (linearized state value function) in formula (13). That is, the obstacle function sub-item described in the embodiment of the present application can be linearized, so as to match the linearized action value function (action value modifier) ​​and the linearized state value function (state value modifier).

[0209] Ginseng Figure 5 , an embodiment of the vehicle trajectory planning device of the present application is introduced. In this embodiment, the vehicle trajectory planning device includes an acquisition module 21, a correction module 22 and a planning module 23.

[0210] The acquisition module 21 is used to acquire the driving trajectory and road boundary information of the target vehicle; the correction module 22 is used to recursively deduce the optimal control rate of each step from the end point of the driving trajectory to the initial point based on the driving trajectory and the linearized action value function to correct the driving trajectory, wherein the action value function includes an obstacle function sub-item, and the obstacle function sub-item is constructed based on the corner point constraints of the target vehicle and the curvature constraints of the driving trajectory based on the road boundary information; the planning module 23 is used to repeatedly recursively deduce the optimal control rate of each step based on the corrected driving trajectory until a preset target is reached to complete the trajectory planning of the target vehicle.

[0211] In one embodiment, the correction module 22 is used to determine a first reference circle that envelops the front corner point of the target vehicle, and a second reference circle that envelops the rear corner point of the target vehicle, wherein the centers of the first reference circle and the second reference circle are located on the axle line of the target vehicle; and based on the collision constraints of the first reference circle and the second reference circle according to the road boundary information, a corner point constraint on the target vehicle is constructed.

[0212] In one embodiment, the correction module 22 is used to perform a minimum constraint on the center position of the first reference circle using the sum of the lower boundary of the road and the radius of the first reference circle, and to perform a maximum constraint on the center position of the first reference circle using the difference between the upper boundary of the road and the radius of the first reference circle; and / or, perform a minimum constraint on the center position of the second reference circle using the sum of the lower boundary of the road and the radius of the second reference circle, and to perform a maximum constraint on the center position of the second reference circle using the difference between the upper boundary of the road and the radius of the second reference circle.

[0213] In one embodiment, the correction module 22 is used to determine the curvature feedforward of the target vehicle at the current step based on the driving trajectory of the target vehicle; and constrain the sum of the curvature of the driving trajectory of the target vehicle at the current step and the curvature feedforward with the maximum turning angle of the target vehicle.

[0214] In one embodiment, the correction module 22 is used to determine the first-order gradient vector and Hessian matrix of the cost function of the corresponding trajectory point and the Jacobian matrix of the system state equation based on the state quantity of the driving trajectory; determine the optimal control rate of the Tth step based on the first-order gradient vector and Hessian matrix of the cost function of the corresponding trajectory point, the Jacobian matrix of the system state equation, and the linearized action value function; determine the state value correction of the Tth step based on the optimal control rate of the Tth step and the linearized action value function, and then recursively deduce the optimal control rate of the T-1th step.

[0215] In one embodiment, the correction module 22 is used to perform a second-order Taylor expansion on the cost function and convert it into a model of trajectory point cost error; perform a first-order Taylor expansion on the system state equation and convert it into a model of trajectory point state quantity error.

[0216] In one embodiment, the optimal control rate includes a control amount correction amount of each trajectory point of the driving trajectory; the correction module 22 is used to correct the control amount of each trajectory point of the driving trajectory based on the control amount correction amount, thereby correcting the driving trajectory.

[0217] As above Figures 1 to 4, a vehicle trajectory planning method according to an embodiment of this specification is described. The details mentioned in the above description of the method embodiment are also applicable to the vehicle trajectory planning device of the embodiment of this specification. The above vehicle trajectory planning device can be implemented by hardware, or by software, or a combination of hardware and software.

[0218] Figure 6 FIG. 1 shows a hardware structure diagram of an unmanned vehicle according to an embodiment of the present specification. Figure 6 As shown, the unmanned vehicle 30 may include at least one processor 31, a memory 32 (e.g., a non-volatile memory), a memory 33, and a communication interface 34, and the at least one processor 31, the memory 32, the memory 33, and the communication interface 34 are connected together via an internal bus 35. At least one processor 31 executes at least one computer-readable instruction stored or encoded in the memory 32.

[0219] It should be understood that the computer executable instructions stored in the memory 32, when executed, cause at least one processor 31 to perform the above combined operations in various embodiments of the present specification. Figures 1 to 4 Describes the various operations and functions.

[0220] In the embodiments of the present specification, the unmanned vehicle 30 can be configured with a functional terminal to carry the above-mentioned hardware structure, and the terminal may include but is not limited to: a personal computer, a server computer, a workstation, a desktop computer, a laptop computer, a notebook computer, a mobile electronic device, a smart phone, a tablet computer, a cellular phone, a personal digital assistant (PDA), a handheld device, a messaging device, a wearable electronic device, a consumer electronic device, and the like.

[0221] According to one embodiment, a program product such as a machine-readable medium is provided. The machine-readable medium may have instructions (i.e., the above-mentioned elements implemented in the form of software), which, when executed by a machine, causes the machine to perform the above-mentioned combination of various embodiments of this specification. Figure 1-Figure 4 Specifically, a system or device equipped with a readable storage medium may be provided, on which a software program code implementing the functions of any of the above-mentioned embodiments is stored, and a computer or processor of the system or device reads and executes the instructions stored in the readable storage medium.

[0222] In this case, the program code itself read from the machine-readable medium can realize the function of any one of the above embodiments, and thus the machine-readable code and the machine-readable storage medium storing the machine-readable code constitute part of this specification.

[0223] Examples of readable storage media include floppy disks, hard disks, magneto-optical disks, optical disks (such as CD-ROM, CD-R, CD-RW, DVD-ROM, DVD-RAM, DVD-RW, DVD-RW), magnetic tapes, non-volatile memory cards, and ROMs. Alternatively, the program code may be downloaded from a server computer or a cloud via a communication network.

[0224] Those skilled in the art should understand that the various embodiments disclosed above can be modified and altered in various ways without departing from the essence of the invention. Therefore, the protection scope of this specification should be defined by the appended claims.

[0225] It should be noted that not all steps and units in the above-mentioned processes and system structure diagrams are necessary, and some steps or units can be ignored according to actual needs. The execution order of each step is not fixed and can be determined as needed. The device structure described in the above-mentioned embodiments can be a physical structure or a logical structure, that is, some units may be implemented by the same physical client, or some units may be implemented by multiple physical clients, or some components in multiple independent devices may be implemented together.

[0226] In the above embodiments, the hardware unit or module can be realized by mechanical or electrical means. For example, a hardware unit, module or processor can include permanent dedicated circuit or logic (such as special processor, FPGA or ASIC) to complete the corresponding operation. The hardware unit or processor can also include programmable logic or circuit (such as general-purpose processor or other programmable processor), which can be temporarily set by software to complete the corresponding operation. Specific implementation (mechanical method or dedicated permanent circuit or temporary circuit) can be determined based on cost and time consideration.

[0227] The specific embodiments described above in conjunction with the accompanying drawings describe exemplary embodiments, but do not represent all embodiments that can be implemented or fall within the scope of protection of the claims. The term "exemplary" used throughout this specification means "used as an example, instance or illustration" and does not mean "preferred" or "having advantages" over other embodiments. For the purpose of providing an understanding of the described technology, the specific embodiments include specific details. However, these technologies can be implemented without these specific details. In some instances, in order to avoid making the concepts of the described embodiments difficult to understand, well-known structures and devices are shown in block diagram form.

[0228] The above description of the present disclosure is provided to enable any person of ordinary skill in the art to implement or use the present disclosure. Various modifications to the present disclosure will be apparent to those of ordinary skill in the art, and the general principles corresponding to the present disclosure may be applied to other variations without departing from the scope of protection of the present disclosure. Therefore, the present disclosure is not limited to the examples and designs described herein, but is consistent with the widest range of principles and novel features disclosed herein.

Claims

1. A vehicle trajectory planning method, characterized in that: The method comprises: Obtain the target vehicle's driving trajectory and road boundary information; Based on the driving trajectory and the linearized action value function, recursively deducing the optimal control rate of each step from the end point of the driving trajectory to the initial point to correct the driving trajectory, wherein the action value function includes an obstacle function sub-item, and the obstacle function sub-item is constructed based on the corner point constraint of the target vehicle and the curvature constraint of the driving trajectory based on the road boundary information; Based on the corrected driving trajectory, the optimal control rate of each step is repeatedly recursively deduced and the driving trajectory is corrected to complete the trajectory planning of the target vehicle.

2. The vehicle trajectory planning method according to claim 1, characterized in that: The method for constructing the barrier function sub-item includes: Determine a first reference circle enveloping a front corner point of the target vehicle, and a second reference circle enveloping a rear corner point of the target vehicle, wherein the centers of the first reference circle and the second reference circle are located on the axle line of the target vehicle; Based on the collision constraints of the road boundary information on the first reference circle and the second reference circle, a corner point constraint on the target vehicle is constructed.

3. The vehicle trajectory planning method according to claim 2, characterized in that: The vehicle trajectory planning method according to claim 1 is characterized in that the first reference circle passes through the front corner point of the target vehicle; and / or the second reference circle passes through the rear corner point of the target vehicle; and / or, Constructing a corner point constraint for the target vehicle based on the collision constraint of the first reference circle and the second reference circle by the road boundary information specifically includes: The center position of the first reference circle is constrained to a minimum value by the sum of the radius of the lower boundary of the road and the first reference circle, and the center position of the first reference circle is constrained to a maximum value by the difference between the radius of the upper boundary of the road and the first reference circle; and / or, The center position of the second reference circle is constrained to a minimum value by the sum of the lower boundary of the road and the radius of the second reference circle, and the center position of the second reference circle is constrained to a maximum value by the difference between the upper boundary of the road and the radius of the second reference circle.

4. The vehicle trajectory planning method according to claim 1, characterized in that: The method for constructing the barrier function sub-item includes: Determining a curvature feedforward of the target vehicle at a current step based on the driving trajectory of the target vehicle; The maximum turning angle of the target vehicle is used to constrain the sum of the curvature of the driving trajectory of the target vehicle at the current step and the curvature feedforward.

5. The vehicle trajectory planning method according to any one of claims 1 to 4, characterized in that: The barrier function sub-item is any one of an exponential function, a logarithmic function, and a square function.

6. The vehicle trajectory planning method according to claim 1, characterized in that: Based on the driving trajectory and the linearized action value function, the optimal control rate of each step is recursively deduced from the end point of the current driving trajectory to the initial point, specifically including: Based on the state quantity of the driving trajectory, determining the first-order gradient vector and the Hessian matrix of the cost function corresponding to the trajectory point, and the Jacobian matrix of the system state equation; Determine the optimal control rate of the Tth step based on the first-order gradient vector and the Hessian matrix of the cost function of the corresponding trajectory point, the Jacobian matrix of the system state equation, and the linearized action value function; Based on the optimal control rate of the Tth step and the linearized action value function, the state value correction amount of the Tth step is determined, and then the optimal control rate of the T-1th step is recursively deduced.

7. The vehicle trajectory planning method according to claim 6, characterized in that: The method further comprises: Performing a second-order Taylor expansion on the cost function and converting it into a model of trajectory point cost error; The system state equation is subjected to first-order Taylor expansion and converted into a model of trajectory point state quantity error.

8. The vehicle trajectory planning method according to claim 7, characterized in that: The optimal control rate includes the control amount correction amount of each track point of the driving track; The method further comprises: Based on the control amount correction amount, the control amount of each trajectory point of the driving trajectory is corrected, thereby correcting the driving trajectory.

9. A vehicle trajectory planning device, characterized in that: include: An acquisition module, used to acquire the driving trajectory and road boundary information of the target vehicle; A correction module, configured to recursively deduce the optimal control rate of each step from the end point of the driving trajectory to the initial point based on the driving trajectory and a linearized action value function, so as to correct the driving trajectory, wherein the action value function includes an obstacle function sub-item, and the obstacle function sub-item is constructed based on the corner point constraint of the target vehicle and the curvature constraint of the driving trajectory based on the road boundary information; The planning module is used to repeatedly recursively deduce the optimal control rate of each step and correct the driving trajectory based on the corrected driving trajectory to complete the trajectory planning of the target vehicle.

10. An unmanned vehicle, characterized in that: include: at least one processor; as well as A memory storing instructions, which, when executed by the at least one processor, enables the at least one processor to execute the vehicle trajectory planning method according to any one of claims 1 to 8.

11. A machine-readable storage medium storing executable instructions, characterized in that: When the instructions are executed, the machine performs the vehicle trajectory planning method as described in any one of claims 1 to 8.

Citation Information

Cited By

  • Vehicle trajectory planning method and device, unmanned vehicle and storage medium

    CN119126813A

  • Vehicle transverse and longitudinal control method and device based on road boundary constraint

    CN120308157A