Vehicle trajectory planning method and device, medium and equipment

Abstract

The invention discloses a vehicle trajectory planning method and device, a medium and equipment. The method comprises the following steps: searching a feasible reference trajectory for a towed device in a vehicle by using a search algorithm; determining a target function, a constraint condition and a system equation; an optimizer is used for solving the system equation, an optimal solution which meets the constraint conditions and enables the total cost calculated according to the objective function to be minimized is obtained, and the optimal solution comprises a first state quantity sequence and a second state quantity sequence; and according to the first state quantity sequence, determining a planned trajectory of the towed equipment, and according to the second state quantity sequence, determining a planned trajectory of the tractor. According to the method, the complexity of the search stage can be effectively reduced, the search difficulty is reduced, the search efficiency is improved, the search success rate is improved, and the complexity and solving time consumption of the whole problem are greatly reduced.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving technology, and in particular to a vehicle trajectory planning method and device, medium, and equipment. Background Art

[0002] In the autonomous driving task, common trajectory planning methods include sampling-based methods, search-based methods, and optimization-based methods. For example, in the port logistics autonomous driving parking scenario, the common vehicle type is a tractor with a trailer. The trailer is the towed equipment. The biggest problem is that the trailer will cut inward during the movement. At this time, the above methods have disadvantages: the sampling-based method usually considers the center point of the rear axle of the tractor as the sampling starting point. If the trailer safety collision detection is introduced, the time consumption will increase exponentially; the search-based method models the trailer kinematics into the search process. Because the trailer model is a follow-up model, it leads to a long search time in complex scenarios; the optimization-based method usually takes the tractor trajectory obtained by sampling or searching as the initial trajectory, and then models an optimization problem. The optimization problem is solved by the optimization algorithm to obtain the optimal control amount of the tractor. The whole process is forward to consider the parking problem, which can be called a forward method. The disadvantages of this method are also obvious. The trailer model is a follow-up model. The search time is long in complex scenarios, so the trajectory solution efficiency is not high.

[0003] Therefore, it is necessary to provide a trajectory planning solution for vehicles including tractors and towed equipment that can reduce time consumption and improve efficiency. Summary of the invention

[0004] In order to solve at least one of the above technical problems, the present invention provides a vehicle trajectory planning method and device, medium, and equipment.

[0005] According to a first aspect, a vehicle trajectory planning method provided by an embodiment of the present invention includes:

[0006] Using a search algorithm to search for a feasible reference trajectory for a towed device in a vehicle; wherein the vehicle includes a towing vehicle and a towed device connected to the towing vehicle;

[0007] Determine an objective function, constraint conditions and system equations; wherein the objective function is to reduce the total cost of the vehicle as an optimization goal, and the total cost includes the offset cost between the driving trajectory of the towed device and the reference trajectory; the system equation is a solution expression for a first state quantity corresponding to the towed device and a second state quantity corresponding to the towing vehicle in the vehicle;

[0008] Solving the system equations using an optimizer to obtain an optimal solution that satisfies the constraint conditions and minimizes the total cost calculated according to the objective function, wherein the optimal solution includes a first state quantity sequence and a second state quantity sequence;

[0009] The planned trajectory of the towed equipment is determined according to the first state quantity sequence, and the planned trajectory of the tractor is determined according to the second state quantity sequence.

[0010] In one embodiment, the total cost also includes an obstacle avoidance cost and a control smoothness cost; wherein, the greater the distance between the vehicle and the obstacle, the smaller the obstacle avoidance cost; the smaller the control amount of the tractor, the smaller the control smoothness cost, and the control amount of the tractor includes the derivative of the curvature of the tractor and the acceleration of the tractor.

[0011] In one embodiment, the objective function J is:

[0012]

[0013] Wherein, N is the number of discrete points in the reference trajectory, k is the sequence number of the discrete point; X ref is the reference state column vector formed by the first state quantity and the second state quantity when the towed device is at each discrete point in the reference trajectory, X is the state column vector formed by the first state quantity and the second state quantity when the towed device travels to each corresponding discrete point; T is the inverted sign; Q is a first diagonal matrix, the diagonal elements corresponding to the first state quantity in the first diagonal matrix are non-zero weights, and the diagonal elements corresponding to the second state quantity in the first diagonal matrix are 0; U is a control column vector formed by the derivative of the curvature of the tractor and the acceleration of the tractor; R is a second diagonal matrix, and the two diagonal elements of the second diagonal matrix are non-zero weights; J obj_lat_dist The cost of keeping away from the obstacle.

[0014] In one embodiment, the first state quantity corresponding to each discrete point includes the derivative of the x-coordinate of the towed device, the derivative of the y-coordinate of the towed device, and the derivative of the heading angle of the towed device at the discrete point; the second state quantity corresponding to each discrete point includes the derivative of the x-coordinate of the tractor, the derivative of the y-coordinate of the tractor, the derivative of the heading angle of the tractor, the derivative of the curvature of the tractor, and the derivative of the speed of the tractor at the discrete point.

[0015] In one embodiment, the system equations include solving the following expressions:

[0016]

[0017]

[0018]

[0019] in, is the derivative of the x-coordinate of the tractor in the second state quantity, is the derivative of the y coordinate of the tractor in the second state quantity, is the derivative of the tractor heading angle in the second state quantity, is the derivative of the curvature of the tractor in the second state quantity, is the derivative of the speed of the tractor in the second state quantity; is the derivative of the x-coordinate of the towed device in the first state quantity, is the derivative of the y coordinate of the towed device in the first state quantity, is the derivative of the heading angle of the towed device in the first state quantity; v is the speed of the tractor, θ1 is the heading angle of the tractor, k is the curvature of the tractor, dk is the derivative of the curvature of the tractor in the control quantity, acc is the acceleration of the tractor in the control quantity; v1 is the speed of the towed device, θ2 is the heading angle of the towed device, L2 is the wheelbase of the towed device, β1 is the difference between the heading angle of the tractor and the heading angle of the towed device, L t It is the length from the center point of the rear axle of the tractor to the hinge point between the towed equipment and the tractor.

[0020] In one embodiment, the speed v1 of the towed device is calculated as follows:

[0021] v1=v 1_f ·cos(β1-α1)

[0022] Among them, v 1_f is the speed of the articulation point, and α1 is the difference between the running direction of the articulation point and the heading angle of the tractor.

[0023] In one embodiment, the calculation formula of the difference α1 between the running direction of the hinge point and the heading angle of the tractor is as follows:

[0024]

[0025] Wherein, δ is the front wheel turning angle of the tractor, and L1 is the wheelbase of the tractor.

[0026] In one embodiment, the velocity v of the hinge point 1_f The calculation formula is as follows:

[0027]

[0028] Wherein, δ is the front wheel turning angle of the tractor, and L1 is the wheelbase of the tractor.

[0029] According to a second aspect, a vehicle trajectory planning device provided by an embodiment of the present invention includes:

[0030] A trajectory search module, used to search a feasible reference trajectory for the towed equipment in the vehicle using a search algorithm; wherein the vehicle includes a tractor and the towed equipment connected to the tractor;

[0031] A first determination module is used to determine an objective function, constraint conditions and a system equation; wherein the objective function is to reduce the total cost of the vehicle as an optimization target, and the total cost includes the offset cost between the driving trajectory of the towed device and the reference trajectory; the system equation is a solution expression for a first state quantity corresponding to the towed device and a second state quantity corresponding to the towing vehicle in the vehicle;

[0032] An optimal solution module, used for solving the system equations by using an optimizer to obtain an optimal solution that satisfies the constraint conditions and minimizes the total cost calculated according to the objective function, wherein the optimal solution includes a first state quantity sequence and a second state quantity sequence;

[0033] The second determination module is used to determine the planned trajectory of the towed equipment according to the first state quantity sequence, and to determine the planned trajectory of the tractor according to the second state quantity sequence.

[0034] According to the third aspect, an embodiment of the present invention provides a computer-readable storage medium, on which a computer program is stored. When the computer program is executed in a computer, the computer is caused to execute the method provided in the first aspect.

[0035] According to a fourth aspect, a computing device provided by an embodiment of the present invention includes a memory and a processor, wherein the memory stores executable code, and when the processor executes the executable code, the method provided by the first aspect is implemented.

[0036] The embodiment of the present invention has the following technical effects: first, an initial reference trajectory is searched for the towed device using a search algorithm, then an objective function, constraint conditions and system equations are established, and then the optimization is used to solve them, thereby obtaining the planned trajectory of the tractor and the towed device, which is a reverse planning idea. When searching for the reference trajectory, the embodiment of the present invention treats the towed device as a separate virtual vehicle, which is less difficult and faster than when the tractor and the towed device are considered at the same time, that is, it can effectively reduce the complexity of the search stage, reduce the search difficulty, and improve the search efficiency. Since the embodiment of the present invention simplifies the trajectory planning problem of the vehicle including the tractor and the towed device, the search success rate can be greatly improved. Moreover, the advantages of the optimizer are fully utilized, and the planned trajectory of the tractor and the towed device is reversely calculated based on the reference trajectory of the towed device obtained by the search, which greatly reduces the complexity of the entire problem and the time consumption of solving the problem. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0038] Figure 1 is a flow chart of a vehicle trajectory planning method in one embodiment of the present invention;

[0039] Figure 2 is a schematic diagram of multiple parameters involved in an embodiment of the present invention;

[0040] Figure 3 is a schematic diagram of multiple tracks involved in one embodiment of the present invention;

[0041] Figure 4 It is a structural block diagram of a vehicle trajectory planning device in one embodiment of the present invention. DETAILED DESCRIPTION

[0042] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution of the present invention will be described clearly and completely below. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work belong to the scope of protection of the present invention.

[0043] In a first aspect, an embodiment of the present invention provides a vehicle trajectory planning method, see Figure 1The method comprises the following steps S110 to S140:

[0044] S110, searching for a feasible reference trajectory for the towed equipment in the vehicle using a search algorithm; wherein the vehicle includes a tractor and the towed equipment connected to the tractor;

[0045] The search algorithm may adopt a hybrid A* algorithm or an A* algorithm. Both the hybrid A* algorithm and the A* algorithm are graph search algorithms, and the hybrid A* algorithm is an improvement made on the basis of the A* algorithm. The difference from the A* algorithm is that the reference path searched by the hybrid A* algorithm takes into account the kinematic constraints of the vehicle, that is, satisfies the maximum curvature constraint of the vehicle, so that the maximum curvature using the hybrid A* algorithm will not exceed the maximum curvature constraint, which conforms to the kinematic law.

[0046] Among them, the vehicle includes a tractor and a towed device. The towed device can be a trailer. The trailer can also be called a trailer. It has no power system and can only move under the drive of the tractor. At this time, the vehicle can be a full-trailer vehicle or a semi-trailer vehicle.

[0047] It can be seen that in S110, the towed device is used as the search starting point, for example, the center of the rear axle of the trailer is used as the search starting point, and a reference trajectory is searched for the towed device using a search algorithm. The reference trajectory is feasible, for example, the reference trajectory does not intersect with obstacles, that is, if the towed device runs according to the reference trajectory, it will not collide with obstacles.

[0048] In actual scenarios, the method provided by the embodiment of the present invention can be applicable to parking scenarios. The reference trajectory obtained by the above search can be a trajectory from the current position of the vehicle to the parking position; of course, it can also be a short trajectory starting from the current position of the vehicle, which has not reached the parking position. During the driving process of the vehicle, the method provided by the embodiment of the present invention is executed once at regular intervals, and the path planning is continuously performed, and finally the vehicle arrives at the parking position accurately.

[0049] S120, determining an objective function, constraint conditions and a system equation; wherein the objective function is to reduce the total cost of the vehicle as an optimization goal, and the total cost includes the offset cost between the driving trajectory of the towed device and the reference trajectory; the system equation is a solution expression for a first state quantity corresponding to the towed device and a second state quantity corresponding to the towing vehicle in the vehicle;

[0050] Among them, the objective function represents the optimization direction of the entire problem. The optimization goal of the objective function is to reduce the total cost of the vehicle, and the total cost includes the offset cost between the driving trajectory of the towed device and the reference trajectory. The smaller the offset between the driving trajectory of the towed device and the reference trajectory, that is, the closer the driving trajectory of the towed device is to the reference trajectory, the smaller the corresponding offset cost is, and the smaller the total cost is.

[0051] Among them, the constraint condition means that the state quantity needs to be limited within a reasonable range during the solution process of the whole problem. The constraint condition can include equality constraint condition and inequality constraint condition. The equality constraint condition can include the constraint condition of the terminal state, the constraint condition of the intermediate position, etc. The inequality constraint condition can include the constraint condition of the curvature of the tractor, the constraint condition of the difference in the heading angle between the tractor and the towed equipment, the constraint condition of the range of the control quantity, etc.

[0052] Among them, the system equation is the rule for state transfer of the optimization problem, so an optimization process represents the rules required to solve and calculate the objective function to achieve the optimization goal under certain constraints. Specifically, the system equation includes a set of solution expressions, which are expressions for solving the state quantity of the towed equipment and the state quantity of the tractor. The state quantity of the towed equipment is called the first state quantity, and the state quantity of the tractor is called the second state quantity. The first state quantity and the second state quantity are the state quantities that need to be solved.

[0053] S130, using an optimizer to solve the system equations to obtain an optimal solution that satisfies the constraint conditions and minimizes the total cost calculated according to the objective function, wherein the optimal solution includes a first state quantity sequence and a second state quantity sequence;

[0054] Among them, the optimizer can adopt an iterative linear quadratic regulator, i.e., iLQR. iLQR is the content of optimal control theory, but because of its fast solution speed and ability to handle nonlinear problems, it is widely used as an optimizer. The trailer kinematics are nonlinear, so it can be quickly solved by iLQR. iLQR is an algorithm for solving the optimal control problem of nonlinear systems. It is an iterative version of the linear quadratic regulator, i.e., LQR. iLQR linearizes the system state equation by performing a first-order Taylor expansion at each time step, and performs a second-order approximation on the cost function, and then uses LQR to solve the approximate problem. By iteratively adjusting the strategy, the optimal strategy is gradually approached. It can be seen that iLQR solves the optimal solution through multiple iterations.

[0055] It is understandable that with the development of autonomous driving technology, optimization algorithms, as an essential component of autonomous driving systems, are developing and applying at a rapid pace. Optimization algorithms belong to the category of parameterized optimization. The core is to concentrate all states in a large matrix, and then use the quasi-Newton method to solve this large matrix. The advantage is that the solution value is more accurate, but the disadvantage is that the calculation efficiency is not fast enough. The optimization algorithm represented by iLQR belongs to the category of scattered point optimization. Each trajectory point can be regarded as a small problem, and then the Bellman optimality principle is used to solve this optimization problem. Because the entire problem is composed of many sub-problems, the solution efficiency is very high.

[0056] It can be seen that after the objective function, constraints and system equations are input into the optimizer, the optimizer obtains the optimal solution after solving the system equations through multiple iterations. The optimal solution not only satisfies the constraints, but also minimizes the total cost calculated according to the objective function as much as possible, that is, achieves the optimization goal.

[0057] Among them, since the trajectory is obtained by connecting multiple discrete points, in order to form a planned trajectory in the subsequent S140, it is necessary to solve the x-coordinates and y-coordinates of multiple discrete points on the planned trajectory, and the relevant information of the x-coordinates and y-coordinates is part of the state quantity to be solved. The optimal solution obtained after the optimizer solves includes a first state quantity sequence and a second state quantity sequence. The first state quantity sequence refers to a sequence formed by the first state quantities corresponding to each of the multiple discrete points on the planned trajectory of the towed equipment, and the second state quantity sequence refers to a sequence formed by the second state quantities corresponding to each of the multiple discrete points on the planned trajectory of the tractor.

[0058] S140. Determine a planned trajectory of the towed equipment according to the first state quantity sequence, and determine a planned trajectory of the tractor according to the second state quantity sequence.

[0059] Since the optimal solution obtained in S130 includes the first state quantity sequence and the second state quantity sequence, the planned trajectory of the towed equipment can be determined based on the relevant information of the x-coordinates and y-coordinates corresponding to each discrete point in the first state quantity sequence, and the planned trajectory of the tractor can be determined based on the relevant information of the x-coordinates and y-coordinates corresponding to each discrete point in the second state quantity sequence, thereby obtaining the planned trajectories of the towed equipment and the tractor respectively.

[0060] In one embodiment, the total cost may also include an obstacle avoidance cost and a control smoothness cost; wherein, the greater the distance between the vehicle and the obstacle, the smaller the obstacle avoidance cost; the smaller the control amount of the tractor, the smaller the control smoothness cost, and the control amount of the tractor includes the derivative of the curvature of the tractor and the acceleration of the tractor.

[0061] It can be seen that the total cost includes not only the offset cost, but also the obstacle avoidance cost and the control smoothness cost. Among them, the control smoothness cost refers to the cost corresponding to the smoothness of the tractor's driving. The smoother the tractor's driving process, the smaller the control smoothness cost. The smoothness of the tractor's driving is reflected by the derivative of the tractor's curvature and the tractor's acceleration. The closer the derivative of the tractor's curvature is to 0, the closer the tractor's acceleration is to 0, and the smoother the tractor's driving process is, so the corresponding control smoothness cost is smaller. The obstacle avoidance cost refers to the cost corresponding to the distance between the vehicle and the obstacle. The farther the vehicle is from the obstacle, the smaller the obstacle avoidance cost. When the distance between the vehicle and the obstacle is greater than a certain distance, the obstacle avoidance cost can be considered to be 0.

[0062] Under the premise of considering the above-mentioned deviation cost, obstacle avoidance cost and control smoothness cost, in one embodiment, the objective function J can be expressed as:

[0063]

[0064] Wherein, N is the number of discrete points in the reference trajectory, k is the sequence number of the discrete point; X ref is the reference state column vector formed by the first state quantity and the second state quantity when the towed device is at each discrete point in the reference trajectory, X is the state column vector formed by the first state quantity and the second state quantity when the towed device travels to each corresponding discrete point; T is the inverted sign; Q is a first diagonal matrix, the diagonal elements corresponding to the first state quantity in the first diagonal matrix are non-zero weights, and the diagonal elements corresponding to the second state quantity in the first diagonal matrix are 0; U is a control column vector formed by the derivative of the curvature of the tractor and the acceleration of the tractor; R is a second diagonal matrix, and the two diagonal elements of the second diagonal matrix are non-zero weights; J obj_lat_dist The cost of keeping away from the obstacle.

[0065] Among them, the non-zero weight can be set according to requirements, for example, set to 1.

[0066] For example, there are three state quantities in the first state quantity and five state quantities in the second state quantity, so X is a column vector with eight elements formed by three state quantities in the first state quantity and five state quantities in the second state quantity, that is, the state column vector mentioned above, and X is the column vector to be solved. Since there are multiple discrete points on the trajectory, each discrete point corresponds to the first state quantity and the second state quantity, and the column vector with eight elements formed by the first state quantity and the second state quantity of each discrete point in the reference trajectory is the reference state column vector X mentioned above. refQ is a diagonal matrix. For example, the size of Q is 8*8. The eight elements on the diagonal of Q correspond to eight state quantities. Since the offset cost of the towed equipment is calculated, the five state quantities in the second state quantity corresponding to the towing vehicle and the corresponding five elements in Q are set to 0, and the three state quantities in the first state quantity corresponding to the towed equipment and the corresponding three elements in Q are set to non-zero values. It can be seen that In fact, the offset cost corresponding to a discrete point.

[0067] Among them, N can be selected by yourself. For example, N is 100, that is, 100 discrete points are uniformly selected on the reference trajectory to participate in the calculation.

[0068] Among them, U T RU is the control smoothness cost of each discrete point. U is a column vector consisting of two elements, which are the derivative of the curvature of the tractor and the acceleration of the tractor. These two elements are also control quantities, because the role of these two elements in the actual scenario is to send them to the chassis control system of the tractor of the vehicle to realize the driving control of the tractor, so these two elements are called control quantities. R is also a diagonal matrix, and the size of R is 2*2. The two elements on the diagonal are non-zero values, and the specific values ​​can be set as needed.

[0069] Among them, J obj_lat_dist It is the cost of keeping away from obstacles. The specific calculation method can be set as needed.

[0070] In one embodiment, the first state quantity corresponding to each discrete point may include the derivative of the x-coordinate of the towed device, the derivative of the y-coordinate of the towed device, and the derivative of the heading angle of the towed device at the discrete point; the second state quantity corresponding to each discrete point may include the derivative of the x-coordinate of the tractor, the derivative of the y-coordinate of the tractor, the derivative of the heading angle of the tractor, the derivative of the curvature of the tractor, and the derivative of the speed of the tractor.

[0071] It can be seen that the first state quantity includes the derivative of the x-coordinate, the derivative of the y-coordinate and the derivative of the heading angle of the towed device, the derivative of the x-coordinate is the relevant information of the x-coordinate, the derivative of the y-coordinate is the relevant information of the y-coordinate, and the derivative of the heading angle of the towed device is the relevant information of the heading angle of the towed device. The first state quantity corresponding to a discrete point includes the relevant information of the x-coordinate, the y-coordinate and the heading angle of the towed device. Therefore, the position and heading of the towed device at this discrete point can be determined based on the first state quantity of a discrete point.

[0072] It can be seen that the second state quantity includes the derivative of the x-coordinate of the tractor, the derivative of the y-coordinate of the tractor, the derivative of the heading angle of the tractor, the derivative of the curvature of the tractor, and the derivative of the speed of the tractor. The second state quantity of a discrete point includes the derivatives of the x-coordinate, the y-coordinate, the heading angle of the tractor, the curvature of the tractor, and the speed of the tractor, that is, their respective related information. According to the x-coordinate, the y-coordinate, and the related information of the heading angle of the tractor, the position and heading of the tractor can be determined. The derivative of the speed of the tractor is the acceleration of the tractor, and the derivative of the curvature of the tractor and the acceleration of the tractor are also used as control quantities, which can be sent to the chassis control system to realize the actual control of the tractor.

[0073] In one embodiment, the system equations include solving the following expressions:

[0074]

[0075] in, is the derivative of the x-coordinate of the tractor in the second state quantity, is the derivative of the y coordinate of the tractor in the second state quantity, is the derivative of the tractor heading angle in the second state quantity, is the derivative of the curvature of the tractor in the second state quantity, is the derivative of the speed of the tractor in the second state quantity; is the derivative of the x-coordinate of the towed device in the first state quantity, is the derivative of the y coordinate of the towed device in the first state quantity, is the derivative of the heading angle of the towed device in the first state quantity; v is the speed of the tractor, θ1 is the heading angle of the tractor, k is the curvature of the tractor, dk is the derivative of the curvature of the tractor in the control quantity, acc is the acceleration of the tractor in the control quantity; v1 is the speed of the towed device, θ2 is the heading angle of the towed device, L2 is the wheelbase of the towed device, β1 is the difference between the heading angle of the tractor and the heading angle of the towed device, L t It is the length from the center point of the rear axle of the tractor to the hinge point between the towed equipment and the tractor.

[0076] It can be seen that the first state quantity includes The second state quantity includes The control quantities include dk and acc. By solving the above system equations using an optimizer, the first state quantity sequence and the second state quantity sequence can be obtained, and of course, the control quantity sequence can also be obtained at the same time.

[0077] In one embodiment, the calculation formula of the speed v1 of the towed device may be as follows:

[0078] v1=v 1_f ·cos(β1-α1)

[0079] Among them, v 1_f is the speed of the articulation point, and α1 is the difference between the running direction of the articulation point and the heading angle of the tractor.

[0080] The calculation formula of the difference α1 between the running direction of the hinge point and the heading angle of the tractor can be as follows:

[0081]

[0082] Wherein, δ is the front wheel turning angle of the tractor, and L1 is the wheelbase of the tractor.

[0083] Wherein, the velocity v of the hinge point 1_f The calculation formula can be as follows:

[0084]

[0085] Wherein, δ is the front wheel turning angle of the tractor, and L1 is the wheelbase of the tractor.

[0086] Among them, the curvature k of the tractor can be expressed as:

[0087] In actual scenarios, if the towed equipment is a trailer, the vehicle may have multiple trailers. Since the accumulation of errors in the actual movement of multiple trailers is uncontrollable, only the planning of the first trailer is considered here.

[0088] See also Figure 2 The various parameters involved in the tractor and the towed equipment are as follows: (x1, y1) is the coordinate of the rear axle center of the tractor, (x2, y2) is the coordinate of the rear axle center of the towed equipment, (x kp ,y kp ) is the coordinate of the hinge point between the tractor and the towed equipment, θ2 is the heading angle of the towed equipment, L2 is the wheelbase of the towed equipment, i.e., the distance from the rear axle center of the towed equipment to the hinge point, v p is the velocity of the hinge point, L t is the distance between the rear axle center and the hinge point of the tractor, R is the turning radius of the rear axle center of the tractor, v is the speed of the tractor in the second state quantity, L1 is the wheelbase of the tractor, that is, the distance between the rear axle center and the front wheel of the tractor, θ1 is the heading angle of the tractor, It is the angle between the tractor body and the towed equipment.

[0089] See also Figure 3 , rectangular box k1 is the towed equipment, rectangular box k2 is the tractor, m1 is the rear axle center of the towed equipment, and m2 is the rear axle center of the tractor. Based on the multiple discrete points obtained by the search algorithm, the multiple discrete points are smoothed to obtain the reference trajectory of the towed equipment, that is, Figure 3 The goal of the optimizer is to make the rear axle center of the towed equipment keep up with this trajectory a. After solving the problem, the optimizer obtains two state quantity sequences, based on which trajectory b and trajectory c can be determined. Trajectory b is the planned trajectory of the towed equipment, and trajectory c is the planned trajectory of the tractor.

[0090] Furthermore, in order to reduce the number of iterations of the optimizer, the optimizer can use hot start to select a trajectory close to the optimization target for the towed device, that is, Figure 3 The trajectory d in , so that the iteration can be started from the trajectory d, instead of starting from a position far away from the optimization target. In this way, the optimizer can reduce the number of iterations, thereby saving iteration time, and can quickly solve the state sequence, thereby quickly obtaining the planning trajectory.

[0091] With regard to the trajectory planning problem of vehicles including tractors and towed equipment, the methods in the prior art all take the tractor as the starting point for sampling, searching or optimization. Among them, the sampling-based method will take exponentially more time if trailer safety collision detection is introduced. Since the towed equipment moves with the tractor, it will sometimes cut inward and sometimes cut outward. The search-based method directly searches for the planned trajectory for the tractor, and the search time is relatively long. The optimization-based method uses the tractor trajectory obtained by sampling or searching as the initial trajectory, and also has the shortcomings of the above two methods. However, the embodiment of the present invention solves the trajectory planning problem with the towed equipment as the starting point. First, a search algorithm is used to search for an initial reference trajectory for the towed equipment, and then the objective function, constraints and system equations are established, and then the optimizer is used to solve them, thereby obtaining the planned trajectories of the tractor and the towed equipment. This is a reverse planning approach. When searching for the reference trajectory of the towed equipment, the problem of obstacle collision is taken into consideration. The feasible reference trajectory will not intersect with the obstacle, and the towed equipment is searched as a separate virtual vehicle. There is no need to consider the internal and external cutting problems caused by following the motion. Therefore, the search time is relatively short, the search efficiency is relatively high, and the efficiency of obtaining the planned trajectory is relatively high.

[0092] It can be seen that the embodiment of the present invention regards the towed device as a virtual vehicle. When searching for the reference trajectory, it is not necessary to consider the following phenomenon of the towed device. Only a search algorithm based on a bicycle model is used for searching. The bicycle model is a simple model, so the embodiment of the present invention can reduce the difficulty of searching. In other words, treating the towed device as a separate virtual vehicle makes the search easier and faster than considering both the towing vehicle and the towed device at the same time. Then, the objective function, constraint conditions and system equations are designed to make the towed device keep up with the reference trajectory as much as possible, and the state quantity is solved by the optimizer. Then, the planned trajectory of the towed device and the towing vehicle can be determined based on the state quantity sequence output by the optimizer. Since the embodiment of the present invention simplifies the trajectory planning problem of the vehicle including the towing vehicle and the towed device, the search success rate can be greatly improved.

[0093] In summary, the reverse planning idea proposed in the embodiment of the present invention can effectively reduce the complexity of the search stage, reduce the difficulty of the search, improve the search success rate, and improve the search efficiency. Moreover, the advantages of the optimizer are fully utilized. Based on the reference trajectory of the towed equipment obtained by the search, the planning trajectory of the tractor and the towed equipment is reversely calculated, reducing the complexity of the entire problem and the time consumption of solving it. In addition, the optimizer solves the control quantity while solving the state quantity. Therefore, the solved control quantity can be used to participate in the control of the tractor, which is very convenient.

[0094] In a second aspect, an embodiment of the present invention provides a vehicle trajectory planning device, see Figure 4 , the device 100 comprises:

[0095] The trajectory search module 110 is used to search a feasible reference trajectory for the towed equipment in the vehicle by using a search algorithm; wherein the vehicle includes a tractor and the towed equipment connected to the tractor;

[0096] The first determination module 120 is used to determine an objective function, constraint conditions and a system equation; wherein the objective function is to reduce the total cost of the vehicle as an optimization target, and the total cost includes the offset cost between the driving trajectory of the towed device and the reference trajectory; the system equation is a solution expression for a first state quantity corresponding to the towed device and a second state quantity corresponding to the towing vehicle in the vehicle;

[0097] An optimal solution module 130, configured to solve the system equations using an optimizer to obtain an optimal solution that satisfies the constraint conditions and minimizes the total cost calculated according to the objective function, wherein the optimal solution includes a first state quantity sequence and a second state quantity sequence;

[0098] The second determination module 140 is used to determine the planned trajectory of the towed equipment according to the first state quantity sequence, and to determine the planned trajectory of the tractor according to the second state quantity sequence.

[0099] In one embodiment, the total cost also includes an obstacle avoidance cost and a control smoothness cost; wherein, the greater the distance between the vehicle and the obstacle, the smaller the obstacle avoidance cost; the smaller the control amount of the tractor, the smaller the control smoothness cost, and the control amount of the tractor includes the derivative of the curvature of the tractor and the acceleration of the tractor.

[0100] In one embodiment, the objective function J is:

[0101]

[0102] Wherein, N is the number of discrete points in the reference trajectory, k is the sequence number of the discrete point; X ref is the reference state column vector formed by the first state quantity and the second state quantity when the towed device is at each discrete point in the reference trajectory, X is the state column vector formed by the first state quantity and the second state quantity when the towed device travels to each corresponding discrete point; T is the inverted sign; Q is a first diagonal matrix, the diagonal elements corresponding to the first state quantity in the first diagonal matrix are non-zero weights, and the diagonal elements corresponding to the second state quantity in the first diagonal matrix are 0; U is a control column vector formed by the derivative of the curvature of the tractor and the acceleration of the tractor; R is a second diagonal matrix, and the two diagonal elements of the second diagonal matrix are non-zero weights; J obj_lat_dist The cost of keeping away from the obstacle.

[0103] In one embodiment, the first state quantity corresponding to each discrete point includes the derivative of the x-coordinate of the towed device, the derivative of the y-coordinate of the towed device, and the derivative of the heading angle of the towed device at the discrete point; the second state quantity corresponding to each discrete point includes the derivative of the x-coordinate of the tractor, the derivative of the y-coordinate of the tractor, the derivative of the heading angle of the tractor, the derivative of the curvature of the tractor, and the derivative of the speed of the tractor at the discrete point.

[0104] In one embodiment, the system equations include solving the following expressions:

[0105]

[0106]

[0107]

[0108] in, is the derivative of the x-coordinate of the tractor in the second state quantity, is the derivative of the y coordinate of the tractor in the second state quantity, is the derivative of the tractor heading angle in the second state quantity, is the derivative of the curvature of the tractor in the second state quantity, is the derivative of the speed of the tractor in the second state quantity; is the derivative of the x-coordinate of the towed device in the first state quantity, is the derivative of the y coordinate of the towed device in the first state quantity, is the derivative of the heading angle of the towed device in the first state quantity; v is the speed of the tractor, θ1 is the heading angle of the tractor, k is the curvature of the tractor, dk is the derivative of the curvature of the tractor in the control quantity, acc is the acceleration of the tractor in the control quantity; v1 is the speed of the towed device, θ2 is the heading angle of the towed device, L2 is the wheelbase of the towed device, β1 is the difference between the heading angle of the tractor and the heading angle of the towed device, L t It is the length from the center point of the rear axle of the tractor to the hinge point between the towed equipment and the tractor.

[0109] In one embodiment, the speed v1 of the towed device is calculated as follows:

[0110] v1=v 1_f ·cos(β1-α1)

[0111] Among them, v 1_f is the speed of the articulation point, and α1 is the difference between the running direction of the articulation point and the heading angle of the tractor.

[0112] In one embodiment, the calculation formula of the difference α1 between the running direction of the hinge point and the heading angle of the tractor is as follows:

[0113]

[0114] Wherein, δ is the front wheel turning angle of the tractor, and L1 is the wheelbase of the tractor.

[0115] In one embodiment, the velocity v of the hinge point 1_f The calculation formula is as follows:

[0116]

[0117] Wherein, δ is the front wheel turning angle of the tractor, and L1 is the wheelbase of the tractor.

[0118] It is understandable that the explanation, specific implementation, beneficial effects, examples, etc. of the relevant contents in the device provided in the embodiment of the present invention can be found in the corresponding parts of the method provided in the first aspect, and will not be repeated here.

[0119] In a third aspect, an embodiment of the present invention provides a computer-readable medium, on which computer instructions are stored. When the computer instructions are executed by a processor, the processor executes the method provided in the first aspect.

[0120] Specifically, a system or device equipped with a storage medium can be provided, on which software program code that implements the functions of any of the above-mentioned embodiments is stored, and a computer (or CPU or MPU) of the system or device can be enabled to read and execute the program code stored in the storage medium.

[0121] In this case, the program code itself read from the storage medium can realize the function of any one of the above-mentioned embodiments, and thus the program code and the storage medium storing the program code constitute a part of the present invention.

[0122] The storage medium embodiments for providing the program code include a floppy disk, a hard disk, a magneto-optical disk, an optical disk (such as CD-ROM, CD-R, CD-RW, DVD-ROM, DVD-RAM, DVD-RW, DVD+RW), a magnetic tape, a non-volatile memory card, and a ROM. Alternatively, the program code can be downloaded from a server computer by a communication network.

[0123] In addition, it should be clear that the functions of any of the above embodiments can be implemented not only by executing the program code read by the computer, but also by enabling an operating system operating on the computer to complete part or all of the actual operations based on instructions from the program code.

[0124] In addition, it can be understood that the program code read from the storage medium is written to a memory provided in an expansion board inserted into the computer or to a memory provided in an expansion module connected to the computer, and then based on the instructions of the program code, a CPU installed on the expansion board or expansion module is enabled to perform part or all of the actual operations, thereby realizing the functions of any of the above-mentioned embodiments.

[0125] It is understandable that the explanation, specific implementation methods, beneficial effects, examples, etc. of the relevant contents in the computer-readable medium provided in the embodiment of the present invention can be found in the corresponding parts of the method provided in the first aspect, and will not be repeated here.

[0126] In a fourth aspect, an embodiment of the present specification provides a computing device, including a memory and a processor, wherein the memory stores executable code, and when the processor executes the executable code, it implements the method in any embodiment of the specification.

[0127] It is understandable that the explanation, specific implementation, beneficial effects, examples, etc. of the relevant contents in the computing device provided in the embodiment of the present invention can be found in the corresponding parts of the method provided in the first aspect, and will not be repeated here.

[0128] It should be noted that the terms used in the present invention are only for describing specific embodiments, rather than limiting the scope of the present application. As shown in the present specification and claims, unless the context clearly indicates an exception, the words "one", "a", "a kind of" and / or "the" do not specifically refer to the singular, but may also include the plural. The terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such a process, method or device. In the absence of further restrictions, the elements defined by the sentence "include one..." do not exclude the presence of other identical elements in the process, method or device including the elements.

[0129] It should also be noted that the terms "center", "up", "down", "left", "right", "vertical", "horizontal", "inside", "outside", etc., indicating the orientation or positional relationship, are based on the orientation or positional relationship shown in the drawings, and are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as a limitation on the present invention. Unless otherwise clearly specified and limited, the terms "installed", "connected", "connected", etc. should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be a direct connection, or it can be an indirect connection through an intermediate medium, or it can be a connection between the two elements. For those of ordinary skill in the art, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.

[0130] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some or all of the technical features therein by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the technical solutions of the embodiments of the present invention.

Claims

1. A vehicle trajectory planning method, characterized in that: include: Using a search algorithm to search for a feasible reference trajectory for a towed device in a vehicle; wherein the vehicle includes a towing vehicle and a towed device connected to the towing vehicle; Determine an objective function, constraint conditions and system equations; wherein the objective function is to reduce the total cost of the vehicle as an optimization goal, and the total cost includes the offset cost between the driving trajectory of the towed device and the reference trajectory; the system equation is a solution expression for a first state quantity corresponding to the towed device and a second state quantity corresponding to the towing vehicle in the vehicle; Solving the system equations using an optimizer to obtain an optimal solution that satisfies the constraint conditions and minimizes the total cost calculated according to the objective function, wherein the optimal solution includes a first state quantity sequence and a second state quantity sequence; The planned trajectory of the towed equipment is determined according to the first state quantity sequence, and the planned trajectory of the tractor is determined according to the second state quantity sequence.

2. The method according to claim 1, characterized in that The total cost also includes an obstacle avoidance cost and a control smoothness cost; wherein, the greater the distance between the vehicle and the obstacle, the smaller the obstacle avoidance cost; the smaller the control amount of the tractor, the smaller the control smoothness cost, and the control amount of the tractor includes the derivative of the curvature of the tractor and the acceleration of the tractor.

3. The method according to claim 2, characterized in that The objective function J is: Wherein, N is the number of discrete points in the reference trajectory, k is the sequence number of the discrete point; X ref is the reference state column vector formed by the first state quantity and the second state quantity when the towed device is at each discrete point in the reference trajectory, X is the state column vector formed by the first state quantity and the second state quantity when the towed device travels to each corresponding discrete point; T is the inverted sign; Q is a first diagonal matrix, the diagonal elements corresponding to the first state quantity in the first diagonal matrix are non-zero weights, and the diagonal elements corresponding to the second state quantity in the first diagonal matrix are 0; U is a control column vector formed by the derivative of the curvature of the tractor and the acceleration of the tractor; R is a second diagonal matrix, and the two diagonal elements of the second diagonal matrix are non-zero weights; J obj_lat_dist The cost of keeping away from the obstacle.

4. The method according to claim 1, characterized in that: The first state quantity corresponding to each discrete point includes the derivative of the x-coordinate of the towed device, the derivative of the y-coordinate of the towed device and the derivative of the heading angle of the towed device when the towed device is at the discrete point; the second state quantity corresponding to each discrete point includes the derivative of the x-coordinate of the tractor when the towed device is at the discrete point, the derivative of the y-coordinate of the tractor, the derivative of the heading angle of the tractor, the derivative of the curvature of the tractor and the derivative of the speed of the tractor.

5. The method according to claim 4, characterized in that The system equations include the following expressions to be solved: in, is the derivative of the x-coordinate of the tractor in the second state quantity, is the derivative of the y coordinate of the tractor in the second state quantity, is the derivative of the tractor heading angle in the second state quantity, is the derivative of the curvature of the tractor in the second state quantity, is the derivative of the speed of the tractor in the second state quantity; is the derivative of the x-coordinate of the towed device in the first state quantity, is the derivative of the y coordinate of the towed device in the first state quantity, is the derivative of the heading angle of the towed device in the first state quantity; v is the speed of the tractor, θ1 is the heading angle of the tractor, k is the curvature of the tractor, dk is the derivative of the curvature of the tractor in the control quantity, acc is the acceleration of the tractor in the control quantity; v1 is the speed of the towed device, θ2 is the heading angle of the towed device, L2 is the wheelbase of the towed device, β1 is the difference between the heading angle of the tractor and the heading angle of the towed device, L t It is the length from the center point of the rear axle of the tractor to the hinge point between the towed equipment and the tractor.

6. The method according to claim 5, characterized in that The calculation formula of the speed v1 of the towed equipment is as follows: v1=v 1_f ·cos(β1-α1) Among them, v 1_f is the speed of the articulation point, and α1 is the difference between the running direction of the articulation point and the heading angle of the tractor.

7. The method according to claim 6, characterized in that The calculation formula of the difference α1 between the running direction of the hinge point and the heading angle of the tractor is as follows: Wherein, δ is the front wheel turning angle of the tractor, and L1 is the wheelbase of the tractor; and / or, the velocity v of the hinge point 1_f The calculation formula is as follows: Wherein, δ is the front wheel turning angle of the tractor, and L1 is the wheelbase of the tractor.

8. A vehicle trajectory planning device, characterized in that: include: A trajectory search module, used to search a feasible reference trajectory for the towed equipment in the vehicle using a search algorithm; wherein the vehicle includes a tractor and the towed equipment connected to the tractor; A first determination module is used to determine an objective function, constraint conditions and a system equation; wherein the objective function is to reduce the total cost of the vehicle as an optimization target, and the total cost includes the offset cost between the driving trajectory of the towed device and the reference trajectory; the system equation is a solution expression for a first state quantity corresponding to the towed device and a second state quantity corresponding to the towing vehicle in the vehicle; An optimal solution module, used for solving the system equations by using an optimizer to obtain an optimal solution that satisfies the constraint conditions and minimizes the total cost calculated according to the objective function, wherein the optimal solution includes a first state quantity sequence and a second state quantity sequence; The second determination module is used to determine the planned trajectory of the towed equipment according to the first state quantity sequence, and to determine the planned trajectory of the tractor according to the second state quantity sequence.

9. A computer-readable storage medium, characterized in that: A computer program is stored thereon, and when the computer program is executed in a computer, the computer is caused to execute the method according to any one of claims 1 to 7.

10. A computing device, characterized in that: The method comprises a memory and a processor, wherein the memory stores executable codes, and when the processor executes the executable codes, the method according to any one of claims 1 to 7 is implemented.

Images