Unmanned driving equipment trajectory planning method, device, electronic device and storage medium

Abstract

The present application discloses a method, device, electronic device and storage medium for trajectory planning of unmanned driving equipment. The method comprises: obtaining a first state of the unmanned driving equipment at a first moment; sampling control quantities according to the first state to obtain multiple groups of sampled control quantities; obtaining a set of sampled states of the unmanned driving equipment at a second moment according to a trajectory reference line, the first state, the multiple groups of sampled control quantities and a prediction model; the prediction model is implemented based on a model predictive control algorithm; dynamic planning is performed according to the first state and the set of sampled states to determine the predicted state of the unmanned driving equipment at the second moment; and the planned trajectory of the unmanned driving equipment is determined according to the first state and the predicted state. The beneficial effect is that the model predictive control MPC is creatively added to the trajectory planning stage, and the sampling is different from the existing scattered sampling in terms of sampling, but the control quantity sampling is used, which provides a guarantee for trajectory accessibility.

Description

Technical Field

[0001] The present application relates to the field of unmanned driving, and specifically to a trajectory planning method, device, electronic device and storage medium for unmanned driving equipment. Background Art

[0002] Unmanned driving has gradually been applied in the fields of logistics, food delivery, etc. The trajectory planning technology of unmanned driving mainly relies on the perception of the environment by the unmanned driving equipment itself, and plans a safe and comfortable operation route by itself, which is the core embodiment of intelligence. At present, the most widely used trajectory planning algorithm is the EM (Expectation Maximization) algorithm, but this algorithm is relatively simple and only imposes a constant upper and lower limit constraint on the curvature constraint. It does not take into account the factors of unmanned driving equipment, so there are disadvantages in its use. Summary of the invention

[0003] In view of the above problems, the present application is proposed to provide an unmanned driving equipment trajectory planning method, device, electronic device and storage medium that overcome the above problems or at least partially solve the above problems.

[0004] According to a first aspect of the present application, a method for trajectory planning of an unmanned driving device is provided, comprising:

[0005] Obtaining a first state of the unmanned driving device at a first moment;

[0006] Sampling the control amount according to the first state to obtain multiple groups of sampled control amounts;

[0007] Obtaining a set of sampled states of the unmanned driving device at a second moment according to the trajectory reference line, the first state, the multiple groups of sampled control quantities and a prediction model; the prediction model is implemented based on a model predictive control algorithm;

[0008] Performing dynamic programming according to the first state and the sampled state set to determine a predicted state of the unmanned driving device at a second moment;

[0009] A planned trajectory of the unmanned driving device is determined according to the first state and the predicted state.

[0010] Optionally, obtaining a set of sampled states of the unmanned driving device at the second moment according to the trajectory reference line, the first state, the multiple groups of sampled control quantities and the prediction model includes:

[0011] For each set of sampled control quantities, a sampling position and a sampling speed of the unmanned driving device at a second moment are determined based on the trajectory reference line, the first state and the prediction model; the sampling state set includes a sampling position subset and a sampling speed subset;

[0012] The performing dynamic programming according to the first state and the sampled state set to determine the predicted state of the unmanned driving device at the second moment includes:

[0013] Determine the predicted position of the unmanned driving device at the second moment according to the first state and the optimal solution of the dynamic programming of the predicted position subset, and determine the predicted speed of the unmanned driving device at the second moment according to the optimal solution of the dynamic programming of the first state and the predicted speed subset;

[0014] The predicted position and the predicted speed are combined to obtain the predicted state of the unmanned driving device at the second moment.

[0015] Optionally, for each group of sampled control quantities, determining the sampling position and sampling speed of the unmanned driving device at the second moment based on the trajectory reference line, the first state and the prediction model respectively includes:

[0016] Determine a first error between the first state and the trajectory reference line based on the trajectory reference line;

[0017] For each group of control quantities, the sampling position error at the second moment and the sampling speed error at the second moment obtained according to the group of control quantities are predicted respectively according to the first state, the first error and the prediction model, and the sampling position and sampling speed of the unmanned driving equipment at the second moment are determined according to the trajectory reference line, the sampling position error and the sampling speed error.

[0018] Optionally, for each group of control quantities, predicting respectively, according to the first state, the first error and the prediction model, a sampling position error at the second moment and a sampling speed error at the second moment obtained according to the group of control quantities comprises:

[0019] According to the group of control variables and the initial error at the first moment, the initial sampling position error and the initial velocity error at the second moment are predicted respectively;

[0020] Performing feedback correction on the initial sampling position error and the initial speed error at the second moment according to the first error, respectively, to obtain the sampling position error at the second moment and the sampling speed error at the second moment;

[0021] The initial error at the start time is determined based on the unmanned driving device.

[0022] Optionally, the sampling of the control amount according to the first state includes:

[0023] Non-uniform sampling is performed according to the first state.

[0024] Optionally, the method further includes: performing secondary planning on the planned trajectory.

[0025] Optionally, performing secondary planning on the planned trajectory includes:

[0026] Constructing a control quantity constraint condition according to an error between the real-time trajectory of the unmanned driving device and the trajectory reference line;

[0027] The planned trajectory is reprogrammed according to the control quantity constraint.

[0028] According to a second aspect of the present application, there is provided an unmanned driving equipment trajectory planning device, comprising:

[0029] An acquisition unit, used to acquire a first state of the unmanned driving device at a first moment;

[0030] A sampling unit, used for sampling the control amount according to the first state to obtain multiple groups of sampled control amounts;

[0031] A planning unit is used to obtain a set of sampling states of the unmanned driving device at a second moment according to a trajectory reference line, the first state, the multiple groups of sampled control quantities and a prediction model; the prediction model is implemented based on a model predictive control algorithm; dynamic planning is performed according to the first state and the set of sampling states to determine the predicted state of the unmanned driving device at the second moment; and a planned trajectory of the unmanned driving device is determined according to the first state and the predicted state.

[0032] Optionally, the planning unit is used to determine, for each group of sampled control quantities, a sampling position and a sampling speed of the unmanned driving device at the second moment based on the trajectory reference line, the first state and the prediction model; the sampling state set includes a sampling position subset and a sampling speed subset; the predicted position of the unmanned driving device at the second moment is determined according to the dynamic programming optimal solution of the first state and the predicted position subset, and the predicted speed of the unmanned driving device at the second moment is determined according to the dynamic programming optimal solution of the first state and the predicted speed subset; the predicted position and predicted speed are combined to obtain the predicted state of the unmanned driving device at the second moment.

[0033] Optionally, the planning unit is used to determine a first error between the first state and the trajectory reference line based on the trajectory reference line; for each group of control quantities, the sampling position error at the second moment and the sampling speed error at the second moment obtained according to the group of control quantities are predicted respectively according to the first state, the first error and the prediction model, and the sampling position and sampling speed of the unmanned driving equipment at the second moment are determined according to the trajectory reference line, the sampling position error and the sampling speed error.

[0034] Optionally, the planning unit is used to predict the initial sampling position error and the initial speed error at the second moment based on the group of control quantities and the initial error at the first moment; and perform feedback correction on the initial sampling position error and the initial speed error at the second moment based on the first error to obtain the sampling position error at the second moment and the sampling speed error at the second moment; wherein the initial error at the starting moment is determined based on the unmanned driving device.

[0035] Optionally, the sampling unit is used to perform non-uniform sampling according to the first state.

[0036] Optionally, the planning unit is further used to perform secondary planning on the planned trajectory.

[0037] Optionally, the planning unit is used to construct a control quantity constraint condition according to the error between the real-time trajectory of the unmanned driving device and the trajectory reference line; and perform secondary planning on the planned trajectory according to the control quantity constraint condition.

[0038] According to a third aspect of the present application, an electronic device is provided, comprising: a processor; and a memory arranged to store computer executable instructions, wherein when the executable instructions are executed, the processor executes any of the methods described above.

[0039] According to a fourth aspect of the present application, a computer-readable storage medium is provided, wherein the computer-readable storage medium stores one or more programs, and when the one or more programs are executed by a processor, they implement any of the methods described above.

[0040] From the above, it can be seen that the technical solution of the present application can predict the sampling state of the unmanned driving device at the next moment corresponding to each sampled control quantity according to the first state of the unmanned driving device at a specified time through control quantity sampling, determine the predicted state of the unmanned driving device at the next moment through dynamic planning, and finally determine the planned trajectory of the unmanned driving device according to the first state and the predicted state. The beneficial effect of this technical solution is that it creatively adds model predictive control MPC to the trajectory planning stage, so that the planned trajectory can be better executed by the control module of the unmanned driving device, that is, the trajectory accessibility is achieved according to the operating characteristics of the unmanned driving device, and the sampling is different from the existing scattered sampling, but the control quantity sampling is used, which further provides a guarantee for the trajectory accessibility, and can provide better technical support for scenarios such as takeout and logistics distribution.

[0041] The above description is only an overview of the technical solution of the present application. In order to more clearly understand the technical means of the present application, it can be implemented in accordance with the contents of the specification. In order to make the above and other purposes, features and advantages of the present application more obvious and easy to understand, the specific implementation methods of the present application are listed below. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] Various other advantages and benefits will become apparent to those of ordinary skill in the art by reading the detailed description of the preferred embodiments below. The accompanying drawings are only for the purpose of illustrating the preferred embodiments and are not to be considered as limiting the present application. Also, the same reference symbols are used throughout the accompanying drawings to represent the same components. In the accompanying drawings:

[0043] Figure 1 A schematic diagram of a process of a trajectory planning method for an unmanned driving device according to an embodiment of the present application is shown;

[0044] Figure 2 A schematic structural diagram of a trajectory planning device for unmanned driving equipment according to an embodiment of the present application is shown;

[0045] Figure 3 A schematic diagram of the structure of an electronic device according to an embodiment of the present application is shown;

[0046] Figure 4 A schematic diagram showing the structure of a computer-readable storage medium according to an embodiment of the present application is shown;

[0047] Figure 5 A schematic diagram of the MPC principle is shown;

[0048] Figure 6 A schematic diagram showing the principle of calculation based on the trajectory reference line. DETAILED DESCRIPTION

[0049] The exemplary embodiments of the present application will be described in more detail below with reference to the accompanying drawings. Although the exemplary embodiments of the present application are shown in the accompanying drawings, it should be understood that the present application can be implemented in various forms and should not be limited by the embodiments set forth herein. On the contrary, these embodiments are provided in order to enable a more thorough understanding of the present application and to fully convey the scope of the present application to those skilled in the art.

[0050] The technical concept of this application is to add MPC (Model Predictive Control) to the trajectory planning process, so that the planned trajectory is more in line with the actual performance of the unmanned driving equipment and has higher trajectory accessibility. MPC is a model-based optimization control algorithm with predictive and explicit constraint processing capabilities. Its principle can be referred to Figure 5 .like Figure 5As shown, MPC mainly includes prediction model, feedback correction, and online finite time domain rolling optimization. Based on the target input and the observed value of the controlled object output, the control amount of the controlled object is output. In the embodiment of the present application, the controlled object can be a controllable component of an unmanned driving device, such as the chassis of a vehicle.

[0051] Among them, predictive control has a prediction function, that is, it can predict the future state of the controlled object based on the control quantity and the observed value output by the controlled object. In the case where the predicted value has errors due to factors such as inaccurate prediction models, feedback correction can be performed through the absolute predicted value of the observed value output by the controlled object. According to the predicted value after target input and feedback correction, the optimal control solution can be obtained in a rolling manner according to the explicitly given constraints and performance requirements. In the prior art, there is no case of applying MPC to the trajectory planning process of unmanned driving equipment.

[0052] The trajectory planning of unmanned driving equipment also has the following characteristics: the result of trajectory planning output is not just a path curve containing position information, but a trajectory connected by a series of trajectory points, and each trajectory point contains information such as position, speed, acceleration, etc. The trajectory points in the trajectory planning of this application can also be called state points, which can also contain information such as position, speed, acceleration, etc., but it is not necessarily required to be directly reflected in the state points. For example, it can be obtained by taking the first or second order derivative of the trajectory.

[0053] The embodiments of the present application can be applied to unmanned driving equipment such as drones and unmanned vehicles. More specifically, the unmanned driving equipment can be automatic delivery equipment used in logistics and food delivery, such as a food delivery robot, which is described below in conjunction with specific embodiments.

[0054] Figure 1 FIG. 1 is a flow chart of a method for trajectory planning of an unmanned driving device according to an embodiment of the present application. Figure 1 As shown, the method includes:

[0055] Step S110, obtaining a first state of the unmanned driving device at a first moment.

[0056] For example, the observed state of the unmanned driving device at the current moment can be determined based on the observed value output by the sensor of the unmanned driving device as the first state at the first moment. At this time, the state of the unmanned driving device at the next moment can be predicted according to the subsequent steps, where the time interval can be a fixed preset value. When planning multiple trajectory points continuously, the state predicted for the specified moment can also be used as the first state at the first moment, and the state at the next moment can be iteratively predicted until the planned number of iterations is met.

[0057] Step S120, sampling the control amount according to the first state to obtain multiple groups of sampled control amounts.

[0058] Control quantities usually include acceleration and steering angle. The steering angle here refers to the steering angle achieved by the control components in the unmanned driving device, for example, the steering angle of the steering wheel. Sampling in the prior art often uses fixed intervals for scattered sampling, which is not consistent with the actual performance and trajectory of the unmanned driving device. The use of control quantity sampling overcomes this shortcoming. In addition, control quantity sampling can also facilitate the decoupling of planning problems, such as separating the three-dimensional optimization problem into path optimization and speed optimization. The following examples will introduce this situation in detail.

[0059] Step S130, obtaining a set of sampled states of the unmanned driving device at the second moment according to the trajectory reference line, the first state, multiple groups of sampled control quantities and a prediction model; the prediction model is implemented based on a model predictive control algorithm.

[0060] An important basis for the trajectory planning of unmanned driving equipment is the trajectory reference line, which is sometimes also called the center line of the road. Its function is to provide a reference for trajectory planning. In an ideal situation without interference from obstacles and other factors, it can be considered as an optimal route. One goal of unmanned driving is to follow the trajectory reference line as much as possible. In this step, the prediction model based on the model predictive control algorithm is mainly used to predict the state according to the trajectory reference line, the first state, and multiple groups of sampled control quantities. Since the prediction model is related to the kinematics of the vehicle body, it can be seen that the sampled states in the obtained sampled state set are what the unmanned driving equipment can achieve based on its own performance. Trajectory planning based on this can meet the trajectory accessibility.

[0061] Step S140, performing dynamic programming based on the first state and the sampled state set to determine the predicted state of the unmanned driving device at the second moment.

[0062] This step mainly determines the optimal solution based on each sampling state and the first state. For example, the first state and each sampling state can be polynomially connected (the polynomial can be a cubic polynomial or a quintic polynomial, etc., and this application does not limit this), and the corresponding curve is fitted, and then the path collision, trajectory smoothness and other factors are considered for the curve, and the curve corresponding to the minimum cost is determined according to the cost function, so as to determine the corresponding sampling state as the predicted state.

[0063] Step S150, determining a planned trajectory of the unmanned driving device according to the first state and the predicted state.

[0064] When planning a trajectory, in addition to the first state and the predicted state, many factors can also be referred to, such as obstacle information, traffic rules, etc. These are also involved in the solutions of the prior art and will not be introduced in detail here. It should be noted that by simply repeating the above steps, the prediction of multiple states can be iteratively realized, so that the planned trajectory is connected by multiple segments of trajectories.

[0065] visible, Figure 1 The method shown can predict the sampling state of the unmanned driving device at the next moment corresponding to each sampled control quantity according to the first state of the unmanned driving device at a specified moment by sampling the control quantity, determine the predicted state of the unmanned driving device at the next moment by dynamic planning, and finally determine the planned trajectory of the unmanned driving device according to the first state and the predicted state. The beneficial effect of this technical solution is that it creatively adds the model predictive control MPC to the trajectory planning stage, so that the planned trajectory can be better executed by the control module of the unmanned driving device, that is, the trajectory accessibility is achieved according to the operating characteristics of the unmanned driving device, and the sampling is different from the existing scattered sampling, but the control quantity sampling is used, which further provides a guarantee for the trajectory accessibility, and can provide better technical support for scenarios such as food delivery and logistics distribution.

[0066] In one embodiment of the present application, in the above method, obtaining a sampling state set of the unmanned driving device at the second moment according to the trajectory reference line, the first state, multiple groups of sampling control quantities and the prediction model includes: for each group of sampling control quantities, determining the sampling position and sampling speed of the unmanned driving device at the second moment based on the trajectory reference line, the first state and the prediction model respectively; the sampling state set includes a sampling position subset and a sampling speed subset; performing dynamic planning according to the first state and the sampling state set to determine the predicted state of the unmanned driving device at the second moment includes: determining the predicted position of the unmanned driving device at the second moment according to the dynamic programming optimal solution of the first state and the predicted position subset, and determining the predicted speed of the unmanned driving device at the second moment according to the dynamic programming optimal solution of the first state and the predicted speed subset; and merging the predicted position and the predicted speed to obtain the predicted state of the unmanned driving device at the second moment.

[0067] This embodiment introduces the specific implementation of separating the three-dimensional optimization problem into path optimization and speed optimization mentioned in the previous embodiment. The first state may specifically include the angle between the current orientation of the unmanned driving device and the trajectory reference line, also known as the heading angle, which is different from the steering angle. For example, in one embodiment, the data format of the first state is [x, y, t, v, heading], where x and y are the position coordinates of the unmanned driving device in the OXY coordinate system, t is the time information, v is the speed, and heading is the heading angle.

[0068] Among them, path optimization is only related to the first state and the steering angle, so the problem of path optimization is to solve a steering angle sequence. Each steering angle in the sequence corresponds to a trajectory point. The optimized path is obtained by connecting the trajectory points in sequence. Similarly, speed optimization is only related to the first state and acceleration. Then the problem of speed optimization is to solve an acceleration sequence. Each acceleration in the sequence corresponds to a trajectory point in the optimized path. The speed can be further solved based on the first state, and then the position, speed and acceleration of each trajectory point are determined by combining them.

[0069] In one embodiment of the present application, in the above method, for each group of sampled control quantities, determining the sampling position and sampling speed of the unmanned driving device at the second moment based on the trajectory reference line, the first state and the prediction model respectively includes: determining the first error between the first state and the trajectory reference line based on the trajectory reference line; for each group of control quantities, predicting the sampling position error at the second moment and the sampling speed error at the second moment obtained according to the group of control quantities according to the first state, the first error and the prediction model respectively, and determining the sampling position and sampling speed of the unmanned driving device at the second moment based on the trajectory reference line, the sampling position error and the sampling speed error.

[0070] Figure 6 Figure 2 shows a schematic diagram of the principle of calculation based on the trajectory reference line. Figure 6 As shown in the figure, the upper curve S is the expected trajectory (i.e., the trajectory reference line). The lower curve is the planned trajectory curve obtained based on the actual running trajectory. Among them, (X, Y) is the trajectory point corresponding to the first state, and the current trajectory point of the vehicle is used as an example in the figure. It can be seen that there are deviations between the two curves, which may be caused by obstacles or unmanned driving equipment performance, model design and other reasons. For the convenience of calculation, it is uniformly called error calculation.

[0071] In the unmanned driving scenario, not only Figure 6 The OXY coordinate system shown in the figure also has a special coordinate system - the Frenet coordinate system - which has also been widely used. The two coordinate systems can be transformed, and the final planning trajectory is often based on the Frenet coordinate system. The horizontal coordinate of the Frenet coordinate system represents the left and right position on the road, also known as the lateral displacement; the vertical coordinate represents the distance along the road, also known as the longitudinal displacement. It can be seen that the advantage of this coordinate system is that the coordinate system is established based on the road, which effectively copes with the characteristics of the curved roads in actual scenarios, which are often not straight.

[0072] After introducing the above background, combined with Figure 6 An example is given to illustrate how to determine a first state and a first error of the trajectory reference line based on the trajectory reference line.

[0073] The important point is to determine the point on the first curve corresponding to the trajectory point of the first state. Figure 6 It is shown as (X des Y des ), i.e., the target trajectory point. Specifically, it is determined by searching for the foot point (the closest point) on the first curve in the first state. is the desired vehicle angular velocity, obtained from the parameters of the desired trajectory:

[0074]

[0075] Among them, V des is the expected vehicle speed determined according to the expected trajectory, K des is the expected road curvature determined according to the expected trajectory. Based on geometric analysis, we can obtain the calculation formula of the lateral error as:

[0076]

[0077] e D is the lateral error, is the lateral error rate, e ψ is the heading error, is the heading error rate; is the vehicle's angular velocity, which can be measured by the vehicle body sensor.

[0078] Longitudinal state calculation, based on geometric analysis, we can obtain the representation of the vehicle's current longitudinal state at the projection point: longitudinal position S p and longitudinal velocity V p , which is the Frenet coordinate system, and the calculation formula is:

[0079]

[0080] According to the current time T of the vehicle, find the trajectory point (X des,T Y des,T ), the longitudinal error state of the vehicle is:

[0081]

[0082] Among them, e S is the longitudinal error, is the longitudinal error rate.

[0083] In summary:

[0084]

[0085] dx and dy are the horizontal and vertical axis errors between the current coordinate point and the coordinate point on the reference line. e1 is the lateral error. is the lateral error rate, e2 is the heading error, is the heading error rate; ψ and S is the current turning angle and angular velocity of the vehicle, which can be measured by the body sensor, and V is the current linear velocity. p and V p is the longitudinal position and longitudinal velocity of the reference line coordinate system. des,T and V des，T is the expected longitudinal position and expected vehicle speed at time T.

[0086] It can be seen that the error can include lateral error and lateral error rate, longitudinal error and longitudinal error rate, heading error and heading error rate.

[0087] According to the prediction model, the error at a certain moment can also be predicted based on the vehicle kinematics principle. Combined with the first error, that is, the observation error determined according to the actual observation value, feedback correction can further improve the accuracy.

[0088] In one embodiment of the present application, in the above method, for each group of control quantities, predicting the sampling position error at the second moment and the sampling speed error at the second moment obtained according to the group of control quantities according to the first state, the first error and the prediction model respectively includes: predicting the initial sampling position error and the initial speed error at the second moment respectively according to the group of control quantities and the initial error at the first moment; performing feedback correction on the initial sampling position error and the initial speed error at the second moment respectively according to the first error to obtain the sampling position error at the second moment and the sampling speed error at the second moment; wherein the initial error at the starting moment is determined according to the unmanned driving equipment.

[0089] Among them, the error can be predicted by the following formula:

[0090]

[0091] Where A, B, and C are the prediction matrices of the MPC prediction model, and D = diag(1 1 … 1). u(k) is the control variable, and x(k) is the prediction error output by the prediction model. m (k) is the actual observed error, and y(k+ 1) is the prediction error of the prediction model after feedback correction.

[0092] In one embodiment of the present application, in the above method, sampling the control amount according to the first state includes: performing non-uniform sampling according to the first state.

[0093] In most cases, the control components of the unmanned driving device will not change significantly, such as the steering wheel will not swing significantly, the acceleration will not change too much, etc. Therefore, the sampling can be densely sampled in the part closer to the first state and sparsely sampled in the part farther away. A specific example is as follows: Use

[0094] θ i =heading±k*i 2

[0095] (Formula 7)

[0096] This quadratic function is used to fit this effect. Assuming that a total of 2n+1 points are sampled, let i = [1,2,3,...,n], and according to Formula 7, we get θ = [θ1,θ2,θ3,...]. θ is the sampled steering angle, heading is the heading angle in the state, and k is the adjustable coefficient.

[0097] Similarly, speed optimization can also be performed in the same way as above, except that the steering angle sampling is replaced by acceleration sampling.

[0098] In one embodiment of the present application, the above method further includes: performing secondary planning on the planned trajectory.

[0099] The planned trajectory obtained in the above embodiment is relatively rough. If further optimization is desired, it can be achieved by using a secondary planning method. For example, in one embodiment of the present application, in the above method, performing secondary planning on the planned trajectory includes: constructing a control quantity constraint condition based on the error between the real-time trajectory of the unmanned driving device and the trajectory reference line; and performing secondary planning on the planned trajectory based on the control quantity constraint condition.

[0100] The vehicle continuous time model is shown as follows:

[0101]

[0102] in Indicates the real-time error of the vehicle; is the control input, δ is the steering angle, a is the acceleration; A, B, C are the prediction matrices.

[0103] Use k-order polynomial to fit the change of control quantity u over time:

[0104]

[0105] The coefficients of the polynomial M = [m0,m1,m2,...,m n ]、N=[n0,n1,n2,...,n n ], the change of the control quantity is determined. Integrate the control error over time [t1, t2]:

[0106]

[0107] Construct the evaluation function:

[0108] z=min ET QE

[0109] Where Q is the coefficient matrix, let x = [MN] T , E T QE can be further transformed into the following form:

[0110] E T QE=x T Hx+x T g+C

[0111] Remove the constant term C, and the final optimized objective function is:

[0112] z=min x T Hx+x T g

[0113] Similarly, the above method can be used to construct constraints, and finally the planning problem can be transformed into a quadratic programming problem in continuous space. The control quantity and optimal trajectory in continuous time can be obtained by using solving methods such as effective sets.

[0114] Figure 2 FIG. 1 is a schematic diagram showing a structure of a trajectory planning device for an unmanned driving device according to an embodiment of the present application. Figure 2 As shown, the unmanned driving equipment trajectory planning device 200 includes:

[0115] The acquisition unit 210 is used to acquire a first state of the unmanned driving device at a first moment.

[0116] For example, the observed state of the unmanned driving device at the current moment can be determined based on the observed value output by the sensor of the unmanned driving device as the first state at the first moment. At this time, the state of the unmanned driving device at the next moment can be predicted according to the subsequent steps, where the time interval can be a fixed preset value. When planning multiple trajectory points continuously, the state predicted for the specified moment can also be used as the first state at the first moment, and the state at the next moment can be iteratively predicted until the planned number of iterations is met.

[0117] The sampling unit 220 is used to sample the control amount according to the first state to obtain multiple groups of sampled control amounts.

[0118] Control quantities usually include acceleration and steering angle. The steering angle here refers to the steering angle achieved by the control components in the unmanned driving device, for example, the steering angle of the steering wheel. Sampling in the prior art often uses fixed intervals for scattered sampling, which is not consistent with the actual performance and trajectory of the unmanned driving device. The use of control quantity sampling overcomes this shortcoming. In addition, control quantity sampling can also facilitate the decoupling of planning problems, such as separating the three-dimensional optimization problem into path optimization and speed optimization. The following examples will introduce this situation in detail.

[0119] The planning unit 230 is used to obtain a set of sampling states of the unmanned driving device at the second moment according to the trajectory reference line, the first state, multiple groups of sampled control quantities and the prediction model; the prediction model is implemented based on the model predictive control algorithm; dynamic planning is performed according to the first state and the set of sampling states to determine the predicted state of the unmanned driving device at the second moment; and the planned trajectory of the unmanned driving device is determined according to the first state and the predicted state.

[0120] An important basis for the trajectory planning of unmanned driving equipment is the trajectory reference line, which is sometimes also called the center line of the road. Its function is to provide a reference for trajectory planning. In an ideal situation without interference from obstacles and other factors, it can be considered as an optimal route. One goal of unmanned driving is to follow the trajectory reference line as much as possible. In this step, the prediction model based on the model predictive control algorithm is mainly used to predict the state according to the trajectory reference line, the first state, and multiple groups of sampled control quantities. Since the prediction model is related to the kinematics of the vehicle body, it can be seen that the sampled states in the obtained sampled state set are what the unmanned driving equipment can achieve based on its own performance. Trajectory planning based on this can meet the trajectory accessibility.

[0121] The optimal solution is determined based on each sampling state and the first state. For example, the first state and each sampling state can be polynomially connected (the polynomial can be a cubic polynomial or a quintic polynomial, etc., and the present application does not impose any restrictions on this), and the corresponding curve is fitted. Then, factors such as path collision and trajectory smoothness are considered for the curve, and the curve corresponding to the minimum cost is determined based on the cost function, so that the corresponding sampling state is determined as the predicted state.

[0122] When planning a trajectory, in addition to the first state and the predicted state, many factors can also be referred to, such as obstacle information, traffic rules, etc. These are also involved in the solutions of the prior art and will not be introduced in detail here. It should be noted that by simply repeating the above steps, the prediction of multiple states can be iteratively realized, so that the planned trajectory is connected by multiple segments of trajectories.

[0123] visible, Figure 2The device shown can predict the sampling state of the unmanned driving device at the next moment corresponding to each sampled control quantity according to the first state of the unmanned driving device at a specified moment by sampling the control quantity, determine the predicted state of the unmanned driving device at the next moment by dynamic planning, and finally determine the planned trajectory of the unmanned driving device according to the first state and the predicted state. The beneficial effect of this technical solution is that it creatively adds the model predictive control MPC to the trajectory planning stage, so that the planned trajectory can be better executed by the control module of the unmanned driving device, that is, the trajectory accessibility is achieved according to the operating characteristics of the unmanned driving device, and the sampling is different from the existing scattered sampling, but the control quantity sampling is used, which further provides a guarantee for the trajectory accessibility, and can provide better technical support for scenarios such as takeout and logistics distribution.

[0124] In one embodiment of the present application, in the above-mentioned device, the planning unit 230 is used to determine the sampling position and sampling speed of the unmanned driving device at the second moment for each group of sampled control quantities based on the trajectory reference line, the first state and the prediction model; the sampling state set includes a sampling position subset and a sampling speed subset; the predicted position of the unmanned driving device at the second moment is determined according to the dynamic programming optimal solution of the first state and the predicted position subset, and the predicted speed of the unmanned driving device at the second moment is determined according to the dynamic programming optimal solution of the first state and the predicted speed subset; the predicted position and the predicted speed are combined to obtain the predicted state of the unmanned driving device at the second moment.

[0125] In one embodiment of the present application, in the above-mentioned device, the planning unit 230 is used to determine the first state and the first error of the trajectory reference line based on the trajectory reference line; for each group of control quantities, the sampling position error at the second moment and the sampling speed error at the second moment obtained according to the group of control quantities are predicted respectively according to the first state, the first error and the prediction model, and the sampling position and sampling speed of the unmanned driving equipment at the second moment are determined according to the trajectory reference line, the sampling position error and the sampling speed error.

[0126] In one embodiment of the present application, in the above-mentioned device, the planning unit 230 is used to predict the initial sampling position error and the initial speed error at the second moment according to the group of control quantities and the initial error at the first moment; and perform feedback correction on the initial sampling position error and the initial speed error at the second moment according to the first error to obtain the sampling position error at the second moment and the sampling speed error at the second moment; wherein the initial error at the starting moment is determined according to the unmanned driving equipment.

[0127] In one embodiment of the present application, in the above device, the sampling unit 220 is used to perform non-uniform sampling according to the first state.

[0128] In one embodiment of the present application, in the above-mentioned device, the planning unit 230 is also used to perform secondary planning on the planned trajectory.

[0129] In one embodiment of the present application, in the above-mentioned device, the planning unit 230 is used to construct a control quantity constraint condition according to the error between the real-time trajectory of the unmanned driving device and the trajectory reference line; and perform secondary planning on the planned trajectory according to the control quantity constraint condition.

[0130] It should be noted that the specific implementation of the above-mentioned device embodiments can be carried out with reference to the specific implementation of the above-mentioned corresponding method embodiments, which will not be repeated here.

[0131] In summary, the technical solution of the present application can predict the sampling state of the unmanned driving device at the next moment corresponding to each sampled control quantity according to the first state of the unmanned driving device at a specified moment by sampling the control quantity, determine the predicted state of the unmanned driving device at the next moment by dynamic planning, and finally determine the planned trajectory of the unmanned driving device according to the first state and the predicted state. The beneficial effect of this technical solution is that it creatively adds model predictive control MPC to the trajectory planning stage, so that the planned trajectory can be better executed by the control module of the unmanned driving device, that is, the trajectory accessibility is achieved according to the operating characteristics of the unmanned driving device, and the sampling is different from the existing scattered sampling, but the control quantity sampling is used, which further provides guarantee for the trajectory accessibility.

[0132] It should be noted that:

[0133] The algorithm and display provided herein are not inherently related to any particular computer, virtual device or other equipment. Various general purpose devices can also be used together with the teachings based on this. According to the above description, it is obvious to construct the structure required for this type of device. In addition, the application is not directed to any specific programming language either. It should be understood that various programming languages ​​can be utilized to realize the content of the application described herein, and the description of the specific language above is for the purpose of disclosing the best mode of the application.

[0134] In the description provided herein, a large number of specific details are described. However, it is understood that the embodiments of the present application can be practiced without these specific details. In some instances, well-known methods, structures and techniques are not shown in detail so as not to obscure the understanding of this description.

[0135] Similarly, it should be understood that in order to streamline the present application and help understand one or more of the various inventive aspects, in the above description of the exemplary embodiments of the present application, the various features of the present application are sometimes grouped together into a single embodiment, figure, or description thereof. However, the disclosed method should not be interpreted as reflecting the following intention: the claimed application requires more features than the features clearly stated in each claim. More specifically, as reflected in the claims below, the inventive aspects are less than all the features of the single embodiment disclosed above. Therefore, the claims following the specific embodiment are hereby expressly incorporated into the specific embodiment, wherein each claim itself serves as a separate embodiment of the present application.

[0136] Those skilled in the art will appreciate that the modules in the devices in the embodiments may be adaptively changed and arranged in one or more devices different from the embodiments. The modules or units or components in the embodiments may be combined into one module or unit or component, and in addition they may be divided into a plurality of submodules or subunits or subcomponents. Except that at least some of such features and / or processes or units are mutually exclusive, all features disclosed in this specification (including the accompanying claims, abstracts and drawings) and all processes or units of any method or device disclosed in this manner may be combined in any combination. Unless otherwise expressly stated, each feature disclosed in this specification (including the accompanying claims, abstracts and drawings) may be replaced by an alternative feature providing the same, equivalent or similar purpose.

[0137] In addition, those skilled in the art will appreciate that, although some embodiments described herein include certain features included in other embodiments but not other features, the combination of features of different embodiments is meant to be within the scope of the present application and form different embodiments. For example, in the claims below, any one of the claimed embodiments may be used in any combination.

[0138] The various component embodiments of the present application can be implemented in hardware, or in software modules running on one or more processors, or in a combination thereof. It should be understood by those skilled in the art that a microprocessor or digital signal processor (DSP) can be used in practice to implement some or all functions of some or all components in the trajectory planning device of the unmanned equipment according to the embodiment of the present application. The present application can also be implemented as a device or device program (e.g., computer program and computer program product) for executing part or all of the methods described herein. Such a program implementing the present application can be stored on a computer-readable medium, or can have the form of one or more signals. Such a signal can be downloaded from an Internet website, or provided on a carrier signal, or provided in any other form.

[0139] For example, Figure 3 A schematic diagram of the structure of an electronic device according to an embodiment of the present application is shown. The electronic device 300 includes a processor 310 and a memory 320 arranged to store computer executable instructions (computer readable program code). The memory 320 can be an electronic memory such as a flash memory, an EEPROM (electrically erasable programmable read-only memory), an EPROM, a hard disk or a ROM. The memory 320 has a storage space 330 for storing a computer readable program code 331 for executing any method step in the above method. For example, the storage space 330 for storing computer readable program code can include various computer readable program codes 331 respectively used to implement various steps in the above method. The computer readable program code 331 can be read from one or more computer program products or written into the one or more computer program products. These computer program products include program code carriers such as a hard disk, a compact disk (CD), a memory card or a floppy disk. Such a computer program product is usually, for example Figure 4 The computer-readable storage medium. Figure 4 A schematic diagram of the structure of a computer-readable storage medium according to an embodiment of the present application is shown. The computer-readable storage medium 400 stores a computer-readable program code 331 for executing the method steps according to the present application, which can be read by the processor 310 of the electronic device 300. When the computer-readable program code 331 is executed by the electronic device 300, the electronic device 300 executes the various steps in the method described above. Specifically, the computer-readable program code 331 stored in the computer-readable storage medium can execute the method shown in any of the above embodiments. The computer-readable program code 331 can be compressed in an appropriate form.

[0140] It should be noted that the above embodiments illustrate the present application rather than limit the present application, and that those skilled in the art may design alternative embodiments without departing from the scope of the appended claims. In the claims, any reference symbol between brackets should not be constructed as a limitation to the claims. The word "comprising" does not exclude the presence of elements or steps not listed in the claims. The word "one" or "an" preceding an element does not exclude the presence of multiple such elements. The present application may be implemented by means of hardware including several different elements and by means of a suitably programmed computer. In a unit claim that lists several devices, several of these devices may be embodied by the same hardware item. The use of the words first, second, and third, etc. does not indicate any order. These words may be interpreted as names.

Claims

1. A method for trajectory planning of an unmanned driving device, comprising: Obtaining a first state of the unmanned driving device at a first moment; Sampling the control amount according to the first state to obtain multiple groups of sampled control amounts; Obtaining a set of sampled states of the unmanned driving device at a second moment according to the trajectory reference line, the first state, the multiple groups of sampled control quantities and a prediction model; the prediction model is implemented based on a model predictive control algorithm; Performing dynamic programming according to the first state and the sampled state set to determine a predicted state of the unmanned driving device at a second moment; Determining a planned trajectory of the unmanned driving device according to the first state and the predicted state; The sampling state set of the unmanned driving device at the second moment obtained according to the trajectory reference line, the first state, the multiple groups of sampled control quantities and the prediction model includes: For each group of sampled control quantities, a sampling position and a sampling speed of the unmanned driving device at a second moment are determined based on the trajectory reference line, the first state and the prediction model; the sampling state set includes a sampling position subset and a sampling speed subset; The performing dynamic programming according to the first state and the sampled state set to determine the predicted state of the unmanned driving device at the second moment includes: Determine the predicted position of the unmanned driving device at the second moment according to the first state and the optimal solution of the dynamic programming of the predicted position subset, and determine the predicted speed of the unmanned driving device at the second moment according to the optimal solution of the dynamic programming of the first state and the predicted speed subset; The predicted position and the predicted speed are combined to obtain the predicted state of the unmanned driving device at the second moment.

2. The method according to claim 1, characterized in that The step of determining the sampling position and the sampling speed of the unmanned driving device at the second moment based on the trajectory reference line, the first state and the prediction model for each group of sampled control quantities comprises: Determine a first error between the first state and the trajectory reference line based on the trajectory reference line; For each group of control quantities, the sampling position error at the second moment and the sampling speed error at the second moment obtained according to the group of control quantities are predicted respectively according to the first state, the first error and the prediction model, and the sampling position and sampling speed of the unmanned driving equipment at the second moment are determined according to the trajectory reference line, the sampling position error and the sampling speed error.

3. The method according to claim 2, characterized in that The step of respectively predicting, for each group of control quantities, a sampling position error at a second moment and a sampling speed error at a second moment obtained according to the group of control quantities according to the first state, the first error and the prediction model comprises: According to the group of control variables and the initial error at the first moment, the initial sampling position error and the initial velocity error at the second moment are predicted respectively; Performing feedback correction on the initial sampling position error and the initial speed error at the second moment according to the first error, respectively, to obtain the sampling position error at the second moment and the sampling speed error at the second moment; The initial error at the start time is determined based on the unmanned driving device.

4. The method according to claim 1, characterized in that The sampling of the control amount according to the first state includes: performing non-uniform sampling according to the first state.

5. The method according to any one of claims 1 to 4, characterized in that The method further includes: performing secondary planning on the planned trajectory.

6. The method according to claim 5, characterized in that The performing secondary planning on the planned trajectory comprises: Constructing a control quantity constraint condition according to an error between the real-time trajectory of the unmanned driving device and the trajectory reference line; The planned trajectory is reprogrammed according to the control quantity constraint.

7. An unmanned driving equipment trajectory planning device, comprising: An acquisition unit, used to acquire a first state of the unmanned driving device at a first moment; A sampling unit, used for sampling the control amount according to the first state to obtain multiple groups of sampled control amounts; A planning unit is used to obtain a set of sampled states of the unmanned driving device at a second moment according to a trajectory reference line, the first state, the multiple sets of sampled control quantities and a prediction model, wherein the set of sampled states of the unmanned driving device at a second moment according to the trajectory reference line, the first state, the multiple sets of sampled control quantities and the prediction model comprises: For each group of sampled control quantities, a sampling position and a sampling speed of the unmanned driving device at a second moment are determined based on the trajectory reference line, the first state and the prediction model; the sampling state set includes a sampling position subset and a sampling speed subset; The performing dynamic programming according to the first state and the sampled state set to determine the predicted state of the unmanned driving device at the second moment includes: Determine the predicted position of the unmanned driving device at the second moment according to the first state and the optimal solution of the dynamic programming of the predicted position subset, and determine the predicted speed of the unmanned driving device at the second moment according to the optimal solution of the dynamic programming of the first state and the predicted speed subset; The predicted position and predicted speed are combined to obtain the predicted state of the unmanned driving device at the second moment; the prediction model is implemented based on the model predictive control algorithm; dynamic planning is performed according to the first state and the sampled state set to determine the predicted state of the unmanned driving device at the second moment; and the planned trajectory of the unmanned driving device is determined according to the first state and the predicted state.

8. An unmanned driving device, characterized in that: The unmanned driving device comprises: a processor; and a memory arranged to store computer executable instructions, wherein when the executable instructions are executed, the processor executes the method according to any one of claims 1 to 6.

9. A computer-readable storage medium, wherein: The computer-readable storage medium stores one or more programs, and when the one or more programs are executed by a processor, the method according to any one of claims 1 to 6 is implemented.

Images