A switching method and device for trajectory planning algorithm

Abstract

The present specification discloses a switching method and device for a trajectory planning algorithm. When an unmanned device switches the trajectory planning algorithm, if the difference between the trajectories calculated by the two trajectory planning algorithms before and after the switching is small, the trajectories calculated by the two trajectory planning algorithms are fused, and the vehicle travels according to the fused trajectory. The trajectory planning algorithm is then switched, and finally the vehicle travels according to the trajectory calculated by the switched trajectory planning algorithm. This effectively improves the stability and smoothness of the unmanned device in switching the trajectory planning algorithm during driving, greatly improves the situation in which the unmanned device experiences severe jitter and loss of stability when switching scenes, and ensures the continuity of scene switching.

Description

Technical Field

[0001] The present application relates to the field of unmanned driving technology, and in particular to a switching method and device for a trajectory planning algorithm. Background Art

[0002] With the continuous advancement of science and technology, unmanned driving technology has gradually matured, and using unmanned equipment to replace manual transportation has become the mainstream trend.

[0003] In existing unmanned equipment technology, unmanned equipment needs to make path planning (PathPlanning) and trajectory planning (Trajectory Planning) during operation. Path planning does not need to consider obstacles, and solves the problem of reaching the destination through a certain route; while trajectory planning needs to consider obstacles, and solves the problem of driving along the planned path while avoiding obstacles. For example, path planning can plan a route from a certain place to an airport, while trajectory planning can plan a trajectory for overtaking while driving.

[0004] During the trajectory planning process, unmanned equipment generally plans a driving route within a period of time by considering location information, perception information of the surrounding environment, etc., and performs specific actions to change the driving state, such as parking, going straight, changing lanes, turning, etc.

[0005] However, in real-world scenarios, the variability of the environment means that unmanned devices cannot use only one trajectory planning algorithm to calculate the trajectory of all scenarios. Therefore, usually for a scenario, the trajectory of the unmanned device is calculated using a trajectory planning algorithm suitable for that scenario. When the scenario changes, in order to adapt to the new scenario, it is necessary to calculate a new trajectory by switching the trajectory planning algorithm.

[0006] However, when the trajectory planning algorithm is switched during driving of the unmanned equipment, the trajectories calculated by different trajectory planning algorithms in the same scenario may be very different, resulting in the unmanned equipment possibly calculating two trajectories that are too different according to the two trajectory planning algorithms. When the unmanned equipment is driving according to the two different trajectories calculated before and after, if the difference between the two trajectories is too large, the unmanned equipment will experience severe shaking, loss of stability, etc., and may even fail to switch the trajectory planning algorithm. Therefore, in the prior art, the unmanned equipment usually needs to stop before switching the trajectory planning algorithm. Summary of the invention

[0007] The embodiments of this specification provide a method and device for switching a trajectory planning algorithm to partially solve the above-mentioned problems existing in the prior art.

[0008] The embodiments of this specification adopt the following technical solutions:

[0009] This specification provides a method for switching a trajectory planning algorithm, wherein the unmanned equipment uses a first trajectory planning algorithm as the currently adopted trajectory planning algorithm, and the method includes:

[0010] When the unmanned equipment is traveling according to the first trajectory calculated by the first trajectory planning algorithm, if it is determined to switch the trajectory planning algorithm, a second trajectory is calculated by the second trajectory planning algorithm;

[0011] determining a first difference value between the first trajectory and the second trajectory;

[0012] If the first difference value is not greater than a preset first threshold, the first trajectory and the second trajectory are fused to obtain an intermediate trajectory, and driving continues according to the intermediate trajectory, and it is determined whether the intermediate trajectory meets a preset condition. If so, the second trajectory planning algorithm is used as the currently adopted trajectory planning algorithm, and driving continues according to the second trajectory. If not, the first trajectory and the second trajectory are re-fused to obtain an intermediate trajectory, until the obtained intermediate trajectory meets the preset condition.

[0013] Optionally, determining a first difference value between the first trajectory and the second trajectory specifically includes:

[0014] Determining the same number of sampling points from the first trajectory and the second trajectory respectively;

[0015] According to the determined sampling points, an average displacement error between the first trajectory and the second trajectory is determined as a first difference value between the first trajectory and the second trajectory.

[0016] Optionally, determining whether the intermediate trajectory meets a preset condition specifically includes:

[0017] Determining the same number of sampling points from the intermediate trajectory and the second trajectory respectively;

[0018] Determine, according to the determined sampling points, an average displacement error between the intermediate trajectory and the second trajectory as a second difference value between the intermediate trajectory and the second trajectory;

[0019] It is determined whether the second difference value is less than a preset second threshold value. If so, it is determined that the intermediate trajectory meets a preset condition. If not, it is determined that the intermediate trajectory does not meet the preset condition.

[0020] Optionally, before fusing the first trajectory and the second trajectory to obtain an intermediate trajectory, the method further includes:

[0021] Determining that the number of times the intermediate trajectory is obtained is no greater than a first preset number of iterations;

[0022] If the number of intermediate trajectories obtained is greater than the first preset number of iterations, driving is stopped, and the second trajectory planning algorithm is used as the currently adopted trajectory planning algorithm, and driving continues according to the second trajectory.

[0023] Optionally, fusing the first trajectory and the second trajectory to obtain an intermediate trajectory specifically includes:

[0024] Adjusting the first trajectory to obtain a fused trajectory;

[0025] Determine a cost function value according to the first trajectory, the second trajectory, the fused trajectory and a preset cost function, wherein the cost function value represents the sum of differences between the fused trajectory and the first trajectory and the second trajectory respectively;

[0026] Determining whether the cost function value is greater than a preset third threshold;

[0027] If so, taking minimizing the cost function value as the optimization goal, readjusting the first trajectory to obtain a fused trajectory, until the cost function value is no greater than the preset third threshold or the number of times the fused trajectory is obtained reaches a second preset number of iterations;

[0028] If not, the obtained fusion trajectory is used as the intermediate trajectory.

[0029] Optionally, determining a cost function value specifically includes:

[0030] Determine a first average displacement error and a first driving direction error between the fused trajectory and the first trajectory; determine a second average displacement error and a second driving direction error between the fused trajectory and the second trajectory;

[0031] determining a first comprehensive difference between the fused trajectory and the first trajectory according to the first average displacement error and the first driving direction error; determining a second comprehensive difference between the fused trajectory and the second trajectory according to the second average displacement error and the second driving direction error;

[0032] According to a first weight corresponding to the first trajectory and a second weight corresponding to the second trajectory, a weighted sum of the first comprehensive difference and the second comprehensive difference is determined as a cost function value.

[0033] Optionally, the first weight is negatively correlated with the number of times the fused trajectory is obtained, and the second weight is positively correlated with the number of times the fused trajectory is obtained.

[0034] Optionally, if the first difference value is greater than a preset first threshold, driving is stopped, and the second trajectory planning algorithm is used as the currently adopted trajectory planning algorithm, and driving is continued along the second trajectory.

[0035] This specification provides a switching device for a trajectory planning algorithm, wherein the device uses a first trajectory planning algorithm as the trajectory planning algorithm currently used by an unmanned device, and the device includes:

[0036] a calculation module, configured to calculate a second trajectory by using a second trajectory planning algorithm when the device is traveling according to the first trajectory calculated by the first trajectory planning algorithm, if it is determined to switch the trajectory planning algorithm;

[0037] a determination module, configured to determine a first difference value between the first trajectory and the second trajectory;

[0038] A judging module, used to judge whether the first difference value is greater than a preset first threshold;

[0039] The first switching module is used to fuse the first trajectory and the second trajectory to obtain an intermediate trajectory when the first difference value is not greater than a preset first threshold value, continue driving along the intermediate trajectory, and determine whether the intermediate trajectory meets a preset condition. If so, the second trajectory planning algorithm is used as the trajectory planning algorithm currently used by the unmanned device to control the unmanned device to continue driving along the second trajectory. If not, the first trajectory and the second trajectory are re-fused to obtain an intermediate trajectory until the obtained intermediate trajectory meets the preset condition.

[0040] The present specification provides a computer-readable storage medium, characterized in that the storage medium stores a computer program, and when the computer program is executed by a processor, a switching method of a trajectory planning algorithm is implemented.

[0041] An unmanned device provided in this specification includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the switching method of the above-mentioned trajectory planning algorithm is implemented when the processor executes the program.

[0042] At least one of the above technical solutions adopted in the embodiments of this specification can achieve the following beneficial effects:

[0043] In this manual, when the unmanned equipment switches the trajectory planning algorithm, if the difference between the trajectories calculated by the two trajectory planning algorithms before and after the switch is small, the trajectories calculated by the two trajectory planning algorithms are fused and the vehicle is driven according to the fused trajectory. The trajectory planning algorithm is then switched, and finally the vehicle is driven according to the trajectory calculated by the switched trajectory planning algorithm. This effectively improves the stability and smoothness of the unmanned equipment in switching the trajectory planning algorithms during driving, greatly improves the situation where the unmanned equipment experiences severe jitter and loss of stability when switching scenes, and ensures the continuity of scene switching. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] The drawings described herein are used to provide a further understanding of the present application and constitute a part of the present application. The illustrative embodiments of the present application and their descriptions are used to explain the present application and do not constitute an improper limitation on the present application. In the drawings:

[0045] Figure 1 A flow chart of a switching method of a trajectory planning algorithm provided in an embodiment of this specification;

[0046] Figure 2 A flow chart of a method for fusing a first trajectory and a second trajectory to obtain an intermediate trajectory provided in an embodiment of this specification;

[0047] Figure 3 A schematic diagram of adjusting a first trajectory to obtain a fused trajectory provided in an embodiment of this specification;

[0048] Figure 4 A schematic diagram of the structure of a device for switching a trajectory planning algorithm provided in an embodiment of this specification;

[0049] Figure 5 The embodiments of this specification provide corresponding to Figure 1 Schematic diagram of unmanned equipment. DETAILED DESCRIPTION

[0050] In order to make the purpose, technical solutions and advantages of this specification more clear, the technical solutions of this application will be clearly and completely described below in combination with the specific embodiments of this specification and the corresponding drawings. Obviously, the described embodiments are only part of the embodiments of this application, not all of them. Based on the embodiments in this specification, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0051] The technical solutions provided by various embodiments of the present application are described in detail below in conjunction with the accompanying drawings. Figure 1 The switching method of the trajectory planning algorithm provided in the embodiment of this specification may specifically include the following steps:

[0052] S100: When the unmanned device is traveling according to the first trajectory calculated by the first trajectory planning algorithm, if it is determined to switch the trajectory planning algorithm, a second trajectory is calculated by a second trajectory planning algorithm.

[0053] In this specification, unmanned equipment mainly includes unmanned vehicles, drones and other intelligent unmanned driving equipment, which are mainly used to replace manual delivery of goods. Unmanned equipment needs to make trajectory planning during driving to avoid obstacles and drive along the planned path.

[0054] Assume that in a certain scenario, the unmanned device uses the first trajectory planning algorithm as the currently used trajectory planning algorithm and drives according to the first trajectory calculated by the first trajectory planning algorithm. During driving, the unmanned device can determine in real time or periodically whether it is necessary to switch the trajectory planning algorithm. If it is determined to switch the trajectory planning algorithm, the second trajectory is calculated using the second trajectory planning algorithm according to the current scenario and the driving information of the unmanned device. The second trajectory planning algorithm described in this specification is the trajectory planning algorithm to which the unmanned device is to switch.

[0055] Specifically, when the unmanned device determines whether it needs to switch the trajectory planning algorithm, it can determine whether a scene switch has occurred based on the current position information or driving information of the unmanned device itself. If a scene switch occurs, it is determined that the trajectory planning algorithm needs to be switched, otherwise it is determined that the trajectory planning algorithm does not need to be switched. For example, different scenes such as highway scenes, urban road scenes, industrial area scenes, commercial area scenes, and residential area scenes can be pre-divided in an electronic map. The unmanned device can then determine whether its current scene has changed based on the current position and the electronic map. If so, it determines the trajectory planning algorithm that needs to be switched based on the changed scene and the preset correspondence between each scene and the trajectory planning algorithm. Otherwise, the current trajectory planning algorithm continues to be used to calculate the trajectory. This manual does not impose any restrictions on the preset division of scenes.

[0056] Furthermore, the unmanned device can also determine whether a scene switch is about to occur. If it is determined that a scene switch is about to occur, it is determined that the trajectory planning algorithm needs to be switched, otherwise it is determined that the trajectory planning algorithm does not need to be switched. Specifically, the unmanned device can determine whether the distance from the current position to the next scene is within the preset distance based on the current position, the current driving trajectory and the electronic map. If so, it determines the trajectory planning algorithm to be switched to based on the next scene and the corresponding relationship between the preset scenes and the trajectory planning algorithm. Otherwise, the current trajectory planning algorithm continues to be used to calculate the trajectory.

[0057] S102: Determine a first difference value between the first trajectory and the second trajectory.

[0058] In the above step S100, after the unmanned device determines to switch to the second trajectory planning algorithm, it does not temporarily switch the currently used trajectory planning algorithm to the second trajectory planning algorithm, but first determines the second trajectory according to the second trajectory planning algorithm. At this time, the unmanned device can obtain two trajectories, namely the first trajectory (current driving trajectory) and the second trajectory (the trajectory to be switched).

[0059] After obtaining the two trajectories, the unmanned device can determine the same number of sampling points from the first trajectory and the second trajectory respectively, and then determine the average displacement error (AverageDisplacementError, ADE) between the first trajectory and the second trajectory according to the determined sampling points, as the first difference value ADE of the first trajectory and the second trajectory. A The first difference value ADE A Characterizes the difference between the first trajectory and the second trajectory.

[0060] Specifically, since there are countless trajectory points in the first trajectory and the second trajectory, it is impossible to determine the displacement errors for all trajectory points one by one, so the same number of sampling points are determined from the first trajectory and the second trajectory respectively. For example, N sampling points are determined from the first trajectory and the second trajectory respectively according to a preset specified distance, or N sampling points are determined from the first trajectory and the second trajectory respectively according to a specified time interval.

[0061] According to the determined sampling points, the displacement error between the ith sampling point in the first track and the ith sampling point in the second track can be determined. The average displacement error of all sampling points is determined as the first difference value ADE between the first track and the second track. A , as shown in formula (1).

[0062]

[0063] In formula (1), x i represents the horizontal coordinate of the i-th sampling point in the first trajectory, represents the horizontal coordinate of the i-th sampling point in the second trajectory, y i represents the ordinate of the i-th sampling point in the first trajectory, represents the ordinate of the i-th sampling point in the second trajectory.

[0064] Furthermore, in actual application scenarios, when a trajectory planning algorithm is used to calculate a trajectory, a number of trajectory points are usually calculated first, and then these trajectory points are connected in sequence to obtain the trajectory. Therefore, in this specification, when the first trajectory planning algorithm and the second trajectory planning algorithm are used to calculate the first trajectory and the second trajectory respectively, if the number of trajectory points calculated for the first trajectory and the number of trajectory points for the second trajectory are the same, the unmanned equipment can directly determine the trajectory points calculated by the two trajectory planning algorithms as sampling points.

[0065] S104: Determine whether the first difference value is greater than a preset first threshold value, if so, execute step S106, otherwise, execute step S108.

[0066] The unmanned device can store a preset first threshold value in advance, and when the first difference value ADE AWhen it is greater than the preset first threshold, it means that the difference between the first trajectory and the second trajectory is too large. If the unmanned equipment directly switches the trajectory planning algorithm during driving, that is, directly switches to the second trajectory during driving, it will cause the stability to be seriously reduced. Therefore, the driving can be stopped, and the second trajectory planning algorithm is used as the currently adopted trajectory planning algorithm, and the driving continues according to the second trajectory, that is, executing step S106.

[0067] When the first difference value ADE A When it is not greater than the preset first threshold, it means that the difference between the first trajectory and the second trajectory is small. The unmanned device can switch the trajectory planning algorithm during driving. In order to further improve the stability during switching, the unmanned device can fuse the first trajectory and the second trajectory to obtain an intermediate trajectory, continue driving according to the intermediate trajectory, and judge whether the intermediate trajectory meets the preset conditions. If so, the second trajectory planning algorithm is used as the currently adopted trajectory planning algorithm, and driving continues according to the second trajectory. If not, the first trajectory and the second trajectory are re-fused to obtain the intermediate trajectory until the obtained intermediate trajectory meets the preset conditions, that is, execute steps S108 to S112.

[0068] S106: Stop driving, use the second trajectory planning algorithm as the currently adopted trajectory planning algorithm, and continue driving along the second trajectory.

[0069] Through the above step S104, when the first difference value ADE is determined A When it is greater than the preset first threshold, it means that the difference between the first trajectory and the second trajectory is too large. If the unmanned equipment directly switches the trajectory planning algorithm during driving, that is, directly switches to the second trajectory during driving, it will cause the stability to be seriously reduced. Therefore, it can be planned to stop driving according to the first trajectory planning algorithm.

[0070] After stopping, the trajectory planning algorithm is switched again, and the second trajectory planning algorithm is used as the currently adopted trajectory planning algorithm, and finally the vehicle continues to travel according to the second trajectory.

[0071] S108: The first trajectory and the second trajectory are integrated to obtain an intermediate trajectory, and the vehicle continues to travel along the intermediate trajectory.

[0072] Through the above step S104, when the first difference value ADE A When it is not greater than the preset first threshold, it indicates that the difference between the first trajectory and the second trajectory is small, and the unmanned device can switch the trajectory planning algorithm during driving. In order to further improve the stability during switching, the unmanned device does not need to stop driving, but can merge the first trajectory and the second trajectory during driving to obtain an intermediate trajectory, and continue driving according to the intermediate trajectory.

[0073] Specifically, the first trajectory may be adjusted by a graph optimization algorithm to obtain an intermediate trajectory. The graph optimization algorithm includes but is not limited to an Elastic Band algorithm.

[0074] S110: Determine whether the intermediate trajectory meets the preset conditions. If so, execute step S112; if not, return to step S108.

[0075] In this specification, when the unmanned device determines whether the intermediate trajectory meets the preset condition, it can determine a second difference value ADE between the intermediate trajectory and the second trajectory. B , if the second difference value ADE B If the second difference value ADE is too large, it means that the difference between the middle track and the second track is too large, and it is not suitable to switch directly from the middle track to the second track during driving. At this time, it means that the middle track does not meet the preset conditions. B If the value is smaller, it means that the difference between the intermediate track and the second track is small, and the intermediate track can be switched directly to the second track during driving. At this time, it means that the intermediate track meets the preset conditions.

[0076] Specifically, the unmanned device can determine the same number of sampling points from the intermediate trajectory and the second trajectory respectively, and then determine the average displacement error between the intermediate trajectory and the second trajectory according to the determined sampling points as the second difference value ADE between the intermediate trajectory and the second trajectory B , determine the second difference value ADE B Is it less than a preset second threshold? If so, it is determined that the intermediate trajectory meets the preset condition, and step S112 is executed. If not, it is determined that the intermediate trajectory does not meet the preset condition, and the process returns to step S108. The method of determining sampling points from the intermediate trajectory and the second trajectory is the same as the method of determining sampling points from the first trajectory and the second trajectory in step S102, and will not be described in detail here.

[0077] S112: Using the second trajectory planning algorithm as the currently adopted trajectory planning algorithm, and continuing driving according to the second trajectory.

[0078] If the second difference between the intermediate trajectory and the second trajectory is less than the preset second threshold, it means that the intermediate trajectory and the second trajectory are relatively close. At this time, the unmanned equipment does not need to stop, and the currently used trajectory planning algorithm can be directly switched to the second trajectory planning algorithm during driving, and the current driving trajectory can be switched from the intermediate trajectory to the second trajectory.

[0079] It should be noted that the unmanned equipment will continuously calculate the trajectory according to the trajectory planning algorithm, the current location of the unmanned equipment, the current road conditions and other information during driving. Therefore, the trajectory calculated each time is different. The process of fusion to obtain the intermediate trajectory shown in the above steps S108 to S112 is essentially an iterative process. If the intermediate trajectory that meets the preset conditions is fused, the vehicle will drive according to the calculated intermediate trajectory. If the intermediate trajectory that meets the preset conditions is not fused, it will return to step S108 to start the next round of iteration, that is, the first trajectory and the second trajectory are re-fused.

[0080] In the above iterative process, the intermediate trajectories fused by the previous and next iterations are also different. This is because: assuming that the first trajectory and the second trajectory based on the previous fusion are calculated by the unmanned device at time t0, and the first trajectory and the second trajectory based on the next fusion are calculated by the unmanned device at time t1, as mentioned above, during the driving process, the unmanned device will continuously calculate the trajectory according to the trajectory planning algorithm, the current location of the unmanned device, the current road conditions and other information, and the location and road conditions of the unmanned device at time t0 and time t1 are different, so the first trajectory and the second trajectory based on which the intermediate trajectory is fused in the previous and next iterations are also different, and the two intermediate trajectories obtained are also different.

[0081] Furthermore, in the iteration process shown in the above steps S108 to S112, the condition for jumping out of the iteration may include not only obtaining an intermediate trajectory that meets the preset condition, but also the number of iterations reaching a first preset number of iterations. That is, before fusing the first trajectory and the second trajectory to obtain the intermediate trajectory in step S108, the unmanned device needs to determine that the number of intermediate trajectories obtained is not greater than the first preset number of iterations.

[0082] If the number of intermediate trajectories obtained is greater than the first preset number of iterations, it means that it is difficult to obtain an intermediate trajectory that is less different from the second trajectory. At this time, the iteration can be stopped, and driving can be stopped. Then the second trajectory planning algorithm is used as the currently adopted trajectory planning algorithm, and driving continues according to the second trajectory, that is, executing step S106.

[0083] Figure 2 A flow chart of a method for fusing a first trajectory and a second trajectory to obtain an intermediate trajectory provided in an embodiment of this specification, such as Figure 2 As shown, the process of fusing the first trajectory and the second trajectory to obtain an intermediate trajectory may specifically include the following steps:

[0084] S1080: According to a preset cost function, the first trajectory is adjusted with minimizing the cost function value as an optimization goal to obtain a fused trajectory.

[0085] Since the first trajectory planning algorithm is essentially an optimization algorithm, in this specification, the unmanned device can minimize the cost function value as the optimization target, adjust at least one parameter in the first trajectory planning algorithm, and then use the first trajectory planning algorithm after adjusting the parameters to calculate the trajectory. The resulting trajectory is called a fused trajectory. The smaller the cost function value, the closer the resulting fused trajectory is to the second trajectory, and vice versa, the closer the resulting fused trajectory is to the first trajectory. The specific form of the cost function will be described below.

[0086] Figure 3 A schematic diagram of adjusting the first trajectory to obtain a fused trajectory provided in an embodiment of this specification, such as Figure 3 As shown, a fused trajectory is obtained during the migration from the first trajectory to the second trajectory, and the fused trajectory is between the first trajectory and the second trajectory.

[0087] S1082: Determine a cost function value according to the first trajectory, the second trajectory, the fused trajectory, and a preset cost function.

[0088] The cost function value represents the sum of the differences between the fusion trajectory and the first trajectory and the second trajectory.

[0089] After the fused trajectory is obtained through step S1080, a first average displacement error and a first driving direction error between the fused trajectory and the first trajectory may be determined; and a second average displacement error and a second driving direction error between the fused trajectory and the second trajectory may be determined.

[0090] Then, a first comprehensive difference between the fused trajectory and the first trajectory is determined according to the first average displacement error and the first driving direction error; and a second comprehensive difference between the fused trajectory and the second trajectory is determined according to the second average displacement error and the second driving direction error.

[0091] As shown in formula (2), the first comprehensive difference f(k) is:

[0092]

[0093] In formula (2), x′ i Represents the horizontal coordinate of the i-th sampling point in the fusion trajectory, y′ i represents the ordinate of the i-th sampling point in the fusion trajectory, β i represents the driving direction of the i-th sampling point in the first trajectory, β′ i Indicates the driving direction of the i-th sampling point in the intermediate trajectory.

[0094] By analogy with formula (2), as shown in formula (3), the second comprehensive difference f(l) is:

[0095]

[0096] In formula (3), represents the driving direction of the i-th sampling point in the second trajectory.

[0097] The method of determining the sampling points from the first trajectory, the second trajectory, and the fused trajectory respectively is the same as the method of determining the sampling points from the first trajectory and the second trajectory in the above step S102, and will not be described in detail here.

[0098] Further, according to the first weight corresponding to the first trajectory and the second weight corresponding to the second trajectory, a weighted sum of the first comprehensive difference and the second comprehensive difference is determined as the cost function value.

[0099] As shown in formula (4), after the first fusion, the cost function cost is:

[0100]

[0101] In formula (4), α1 is the initial value of the first weight, and α2 is the initial value of the second weight.

[0102] Furthermore, the first weight is negatively correlated with the number of times the fused trajectory is obtained, and the second weight is positively correlated with the number of times the fused trajectory is obtained, that is, as the number of iterations of steps S1080 to S1086 gradually increases, the first weight gradually decreases, and the second weight gradually increases, thereby obtaining formula (5).

[0103]

[0104] In formula (5), α A is the first weight, α B is the second weight.

[0105] With the minimization of the cost function value as the optimization goal, at least one parameter in the first trajectory planning algorithm is adjusted so that the value of the cost function cost gradually decreases. Since when the above formula (5) is used as the cost function, the value of the cost function cost will be smaller only when the fused trajectory is closer to the second trajectory (that is, the smaller f(l) is), therefore, with the minimization of the cost function value as the optimization goal, when the first trajectory planning algorithm is adjusted, in the process of continuous iteration, the first comprehensive difference f(k) gradually increases, the fused trajectory gradually deviates from the first trajectory, the second comprehensive difference f(l) gradually decreases, and the fused trajectory gradually approaches the second trajectory, so that the fused trajectory gradually migrates from the first trajectory to the second trajectory, and finally a fused trajectory that is close enough to the second trajectory is obtained.

[0106] Furthermore, in actual application scenarios, a distance weight γ can be set for each sampling point, and the distance weight can be stored in the unmanned device. Specifically, the distance weight γ of each sampling point can be preset according to the distance between the sampling point and the current position of the unmanned device. The closer the sampling point is to the current position of the unmanned device, the greater the distance weight γ, and the farther the sampling point is from the current position of the unmanned device, the smaller the distance weight γ.

[0107] Therefore, as shown in formula (6), the cost function cost can be expressed as:

[0108]

[0109] In formula (6), γ k is the distance weight between the first trajectory and the kth sampling point in the fusion trajectory, γ l is the distance weight between the fused trajectory and the lth sampling point in the second trajectory.

[0110] In addition, in this specification, f(l) may also be directly used as the cost function. In this case, the first trajectory may still be adjusted by minimizing the cost function value to obtain a fused trajectory that is sufficiently close to the second trajectory.

[0111] S1084: Determine whether the cost function value is greater than a preset third threshold value. If so, return to execute step S1080; otherwise, execute step S1086.

[0112] The unmanned device may pre-store a preset third threshold. When the cost function value is greater than the preset third threshold, it indicates that the difference between the fused trajectory and the second trajectory is too large. In order to make the fused trajectory between the first trajectory and the second trajectory and gradually migrate to the second trajectory, the unmanned device needs to re-adjust the first trajectory according to the preset cost function to minimize the cost function value as the optimization target, and obtain the fused trajectory, that is, return to execute step S1080.

[0113] When the cost function value is not greater than the preset third threshold, it means that the difference between the fused trajectory and the first trajectory and the second trajectory is small, and the fused trajectory is between the first trajectory and the second trajectory. The unmanned device can use the obtained fused trajectory as the intermediate trajectory, that is, execute step S1086.

[0114] S1086: Using the obtained fusion trajectory as the intermediate trajectory.

[0115] Furthermore, in the iterative process shown in the above steps S1080 to S1086, the condition for jumping out of the iteration may include not only obtaining a fused trajectory satisfying a cost function value not greater than a preset third threshold, but also obtaining a fused trajectory when the number of times has reached a second preset number of iterations.

[0116] If the number of times the fused trajectory is obtained is greater than the second preset number of iterations, the iteration is stopped, and the fused trajectory obtained by the last iteration is used as the intermediate trajectory, that is, step S1086 is executed.

[0117] Figure 4 A schematic diagram of a device structure for switching a trajectory planning algorithm provided in an embodiment of this specification, wherein the device uses a first trajectory planning algorithm as the trajectory planning algorithm currently used by the unmanned device, and the device includes:

[0118] A calculation module 401, configured to calculate a second trajectory using a second trajectory planning algorithm when the device is traveling according to the first trajectory calculated by the first trajectory planning algorithm, if it is determined to switch the trajectory planning algorithm;

[0119] A determination module 402, configured to determine a first difference value between the first trajectory and the second trajectory;

[0120] A judging module 403 is used to judge whether the first difference value is greater than a preset first threshold;

[0121] The first switching module 404 is used to fuse the first trajectory and the second trajectory to obtain an intermediate trajectory when the first difference value is not greater than a preset first threshold value, continue driving along the intermediate trajectory, and determine whether the intermediate trajectory meets a preset condition. If so, the second trajectory planning algorithm is used as the trajectory planning algorithm currently used by the unmanned device to control the unmanned device to continue driving along the second trajectory. If not, the first trajectory and the second trajectory are re-fused to obtain an intermediate trajectory until the obtained intermediate trajectory meets the preset condition.

[0122] Optionally, the determination module 402 is specifically configured to determine the same number of sampling points from the first trajectory and the second trajectory respectively; and determine an average displacement error between the first trajectory and the second trajectory according to the determined sampling points as a first difference value between the first trajectory and the second trajectory.

[0123] Optionally, the judgment module 403 is specifically configured to determine the same number of sampling points from the intermediate trajectory and the second trajectory respectively; determine, according to the determined sampling points, an average displacement error between the intermediate trajectory and the second trajectory as a second difference value between the intermediate trajectory and the second trajectory; and judge whether the second difference value is less than a preset second threshold value, if so, determine that the intermediate trajectory meets the preset condition; if not, determine that the intermediate trajectory does not meet the preset condition.

[0124] Optionally, the first switching module 404 is further used to, before fusing the first trajectory and the second trajectory to obtain an intermediate trajectory, determine that the number of times the intermediate trajectory is obtained is not greater than a first preset number of iterations; if the number of times the intermediate trajectory is obtained is greater than the first preset number of iterations, stop driving, and use the second trajectory planning algorithm as the currently adopted trajectory planning algorithm, and continue driving according to the second trajectory.

[0125] Optionally, the first switching module 404 is specifically used to adjust the first trajectory to obtain a fused trajectory; determine a cost function value according to the first trajectory, the second trajectory, the fused trajectory and a preset cost function, wherein the cost function value represents the sum of the differences between the fused trajectory and the first trajectory and the second trajectory respectively; judge whether the cost function value is greater than a preset third threshold; if so, readjust the first trajectory with minimizing the cost function value as an optimization goal to obtain the fused trajectory until the cost function value is no greater than the preset third threshold or the number of times the fused trajectory is obtained reaches a second preset number of iterations; if not, use the obtained fused trajectory as the intermediate trajectory.

[0126] Optionally, the first switching module 404 is specifically used to determine a first average displacement error and a first driving direction error between the fused trajectory and the first trajectory; determine a second average displacement error and a second driving direction error between the fused trajectory and the second trajectory; determine a first comprehensive difference between the fused trajectory and the first trajectory based on the first average displacement error and the first driving direction error; determine a second comprehensive difference between the fused trajectory and the second trajectory based on the second average displacement error and the second driving direction error; and determine a weighted sum of the first comprehensive difference and the second comprehensive difference as a cost function value based on a first weight corresponding to the first trajectory and a second weight corresponding to the second trajectory.

[0127] Optionally, the first weight is negatively correlated with the number of times the fused trajectory is obtained, and the second weight is positively correlated with the number of times the fused trajectory is obtained.

[0128] Optionally, the device also includes: a second switching module 405, which is used to control the unmanned equipment to stop driving when the first difference value is greater than a preset first threshold value, and use the second trajectory planning algorithm as the trajectory planning algorithm currently used by the unmanned equipment to control the unmanned equipment to continue driving along the second trajectory.

[0129] The present specification also provides a computer-readable storage medium, which stores a computer program, which can be used to execute the above Figure 1 A switching method for the provided trajectory planning algorithm.

[0130] based on Figure 1 The switching method of the trajectory planning algorithm shown in the present specification also proposes Figure 5 The schematic structure diagram of the unmanned equipment is shown in FIG. Figure 5 At the hardware level, the electronic device includes a processor, an internal bus, a network interface, a memory, and a non-volatile memory, and may also include other hardware required for the business. The processor reads the corresponding computer program from the non-volatile memory into the memory and then runs it to achieve the above Figure 1 The switching method of the trajectory planning algorithm.

[0131] Of course, in addition to software implementation, this specification does not exclude other implementation methods, such as logic devices or a combination of software and hardware, etc., that is to say, the executor of the following processing flow is not limited to each logic unit, but can also be hardware or logic devices.

[0132] In the 1990s, improvements to a technology could be clearly distinguished as hardware improvements (for example, improvements to the circuit structure of diodes, transistors, switches, etc.) or software improvements (improvements to the method flow). However, with the development of technology, many improvements to the method flow today can be regarded as direct improvements to the hardware circuit structure. Designers almost always obtain the corresponding hardware circuit structure by programming the improved method flow into the hardware circuit. Therefore, it cannot be said that an improvement in a method flow cannot be implemented using a hardware entity module. For example, a programmable logic device (PLD) (such as a field programmable gate array (FPGA)) is such an integrated circuit whose logical function is determined by the user's programming of the device. Designers can "integrate" a digital system on a PLD by programming it themselves, without having to ask a chip manufacturer to design and produce a dedicated integrated circuit chip. Moreover, nowadays, instead of manually making integrated circuit chips, this kind of programming is mostly implemented by "logic compiler" software, which is similar to the software compiler used when developing and writing programs, and the original code before compilation must also be written in a specific programming language, which is called hardware description language (HDL). There is not only one HDL, but many kinds, such as ABEL (Advanced Boolean Expression Language), AHDL (Altera Hardware Description Language), Confluence, CUPL (Cornell University Programming Language), HDCal, JHDL (Java Hardware Description Language), Lava, Lola, MyHDL, PALASM, RHDL (Ruby Hardware Description Language), etc. The most commonly used ones are VHDL (Very-High-Speed ​​Integrated Circuit Hardware Description Language) and Verilog. Those skilled in the art should also know that it is only necessary to program the method flow slightly in the above-mentioned hardware description languages ​​and program it into the integrated circuit, and then it is easy to obtain the hardware circuit that implements the logic method flow.

[0133] The controller can be implemented in any appropriate manner, for example, the controller can take the form of a microprocessor or processor and a computer-readable medium storing a computer-readable program code (such as software or firmware) that can be executed by the (micro)processor, a logic gate, a switch, an application-specific integrated circuit (ASIC), a programmable logic controller, and an embedded microcontroller. Examples of controllers include, but are not limited to, the following microcontrollers: ARC 625D, Atmel AT91SAM, Microchip PIC18F26K20, and Silicone Labs C8051F320. The memory controller can also be implemented as part of the control logic of the memory. Those skilled in the art also know that in addition to implementing the controller in a purely computer-readable program code manner, the controller can be implemented in the form of a logic gate, a switch, an application-specific integrated circuit, a programmable logic controller, and an embedded microcontroller by logically programming the method steps. Therefore, this controller can be considered as a hardware component, and the devices included therein for implementing various functions can also be regarded as structures within the hardware component. Or even, the devices for implementing various functions can be regarded as both software modules for implementing the method and structures within the hardware component.

[0134] The systems, devices, modules or units described in the above embodiments may be implemented by computer chips or entities, or by products with certain functions. A typical implementation device is a computer. Specifically, the computer may be, for example, a personal computer, a laptop computer, a cellular phone, a camera phone, a smart phone, a personal digital assistant, a media player, a navigation device, an email device, a game console, a tablet computer, a wearable device, or a combination of any of these devices.

[0135] For the convenience of description, the above device is described in various units according to their functions. Of course, when implementing this specification, the functions of each unit can be implemented in the same or multiple software and / or hardware.

[0136] Those skilled in the art will appreciate that embodiments of the present invention may be provided as methods, systems, or computer program products. Therefore, the present invention may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Moreover, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0137] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0138] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0139] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0140] In a typical configuration, a computing device includes one or more processors (CPU), input / output interfaces, network interfaces, and memory.

[0141] The memory may include non-permanent storage in a computer-readable medium, random access memory (RAM) and / or non-volatile memory in the form of read-only memory (ROM) or flash RAM. The memory is an example of a computer-readable medium.

[0142] Computer readable media include permanent and non-permanent, removable and non-removable media that can be implemented by any method or technology to store information. Information can be computer readable instructions, data structures, program modules or other data. Examples of computer storage media include, but are not limited to, phase change memory (PRAM), static random access memory (SRAM), dynamic random access memory (DRAM), other types of random access memory (RAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), flash memory or other memory technology, compact disk read-only memory (CD-ROM), digital versatile disk (DVD) or other optical storage, magnetic cassettes, magnetic tape magnetic disk storage or other magnetic storage devices or any other non-transmission media that can be used to store information that can be accessed by a computing device. As defined herein, computer readable media does not include temporary computer readable media (transitory media), such as modulated data signals and carrier waves.

[0143] It should also be noted that the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, commodity 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 process, method, commodity or device. In the absence of more restrictions, the elements defined by the sentence "comprises a ..." do not exclude the existence of other identical elements in the process, method, commodity or device including the elements.

[0144] Those skilled in the art will appreciate that the embodiments of this specification may be provided as methods, systems or computer program products. Therefore, this specification may take the form of a complete hardware embodiment, a complete software embodiment or an embodiment combining software and hardware. Moreover, this specification may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0145] This specification may be described in the general context of computer-executable instructions executed by a computer, such as program modules. Generally, program modules include routines, programs, objects, components, data structures, etc. that perform specific tasks or implement specific abstract data types. This specification may also be practiced in distributed computing environments where tasks are performed by remote processing devices connected through a communication network. In a distributed computing environment, program modules may be located in local and remote computer storage media, including storage devices.

[0146] Each embodiment in this specification is described in a progressive manner, and the same or similar parts between the embodiments can be referred to each other, and each embodiment focuses on the differences from other embodiments. In particular, for the system embodiment, since it is basically similar to the method embodiment, the description is relatively simple, and the relevant parts can be referred to the partial description of the method embodiment.

[0147] The above description is only an embodiment of the present specification and is not intended to limit the present specification. For those skilled in the art, the present specification may have various changes and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present specification shall be included in the scope of the claims of the present specification.

Claims

1. A switching method for a trajectory planning algorithm, characterized in that: The unmanned device uses the first trajectory planning algorithm as the currently adopted trajectory planning algorithm, and the method includes: When the unmanned equipment is traveling according to the first trajectory calculated by the first trajectory planning algorithm, if it is determined to switch the trajectory planning algorithm, a second trajectory is calculated by the second trajectory planning algorithm; determining a first difference value between the first trajectory and the second trajectory; If the first difference value is not greater than a preset first threshold value, the first trajectory and the second trajectory are merged to obtain an intermediate trajectory, and the vehicle continues to travel along the intermediate trajectory, and it is determined whether the intermediate trajectory meets a preset condition. If so, the second trajectory planning algorithm is used as the currently adopted trajectory planning algorithm, and the vehicle continues to travel along the second trajectory. If not, the first trajectory and the second trajectory are re-fused to obtain an intermediate trajectory until the intermediate trajectory meets the preset condition. The fusion of the first trajectory and the second trajectory to obtain the intermediate trajectory specifically includes: adjusting the first trajectory according to a preset cost function, taking minimizing the cost function value as an optimization target, and obtaining a fused trajectory; Determine a cost function value according to the first trajectory, the second trajectory, the fused trajectory and a preset cost function, wherein the cost function value represents the sum of differences between the fused trajectory and the first trajectory and the second trajectory respectively; Determining whether the cost function value is greater than a preset third threshold; If so, taking minimizing the cost function value as the optimization goal, readjusting the first trajectory to obtain a fused trajectory, until the cost function value is no greater than the preset third threshold or the number of times the fused trajectory is obtained reaches a second preset number of iterations; If not, the obtained fusion trajectory is used as the intermediate trajectory.

2. The method according to claim 1, characterized in that Determining a first difference value between the first trajectory and the second trajectory specifically includes: Determining the same number of sampling points from the first trajectory and the second trajectory respectively; According to the determined sampling points, an average displacement error between the first trajectory and the second trajectory is determined as a first difference value between the first trajectory and the second trajectory.

3. The method according to claim 1, characterized in that Determining whether the intermediate trajectory satisfies a preset condition specifically includes: determining the same number of sampling points from the intermediate trajectory and the second trajectory respectively; Determine, according to the determined sampling points, an average displacement error between the intermediate trajectory and the second trajectory as a second difference value between the intermediate trajectory and the second trajectory; It is determined whether the second difference value is less than a preset second threshold value. If so, it is determined that the intermediate trajectory meets a preset condition. If not, it is determined that the intermediate trajectory does not meet the preset condition.

4. The method according to claim 1, characterized in that Before fusing the first trajectory and the second trajectory to obtain an intermediate trajectory, the method further includes: Determining that the number of times the intermediate trajectory is obtained is no greater than a first preset number of iterations; If the number of intermediate trajectories obtained is greater than the first preset number of iterations, driving is stopped, and the second trajectory planning algorithm is used as the currently adopted trajectory planning algorithm, and driving continues according to the second trajectory.

5. The method according to claim 1, characterized in that Determine the cost function value, including: Determine a first average displacement error and a first driving direction error between the fused trajectory and the first trajectory; determine a second average displacement error and a second driving direction error between the fused trajectory and the second trajectory; determining a first comprehensive difference between the fused trajectory and the first trajectory according to the first average displacement error and the first driving direction error; determining a second comprehensive difference between the fused trajectory and the second trajectory according to the second average displacement error and the second driving direction error; According to a first weight corresponding to the first trajectory and a second weight corresponding to the second trajectory, a weighted sum of the first comprehensive difference and the second comprehensive difference is determined as a cost function value.

6. The method according to claim 5, characterized in that The first weight is negatively correlated with the number of times the fused trajectory is obtained, and the second weight is positively correlated with the number of times the fused trajectory is obtained.

7. The method according to claim 1, characterized in that If the first difference value is greater than a preset first threshold, the method further includes: Stop driving, use the second trajectory planning algorithm as the currently adopted trajectory planning algorithm, and continue driving along the second trajectory.

8. A switching device for a trajectory planning algorithm, characterized in that: The device uses the first trajectory planning algorithm as the trajectory planning algorithm currently used by the unmanned equipment, and the device includes: a calculation module, configured to calculate a second trajectory by using a second trajectory planning algorithm when the device is traveling according to the first trajectory calculated by the first trajectory planning algorithm, if it is determined to switch the trajectory planning algorithm; a determination module, configured to determine a first difference value between the first trajectory and the second trajectory; A judging module, used to judge whether the first difference value is greater than a preset first threshold; The first switching module is used for fusing the first trajectory and the second trajectory to obtain an intermediate trajectory when the first difference value is not greater than a preset first threshold value, continuing to travel along the intermediate trajectory, and judging whether the intermediate trajectory meets a preset condition. If so, the second trajectory planning algorithm is used as the trajectory planning algorithm currently adopted by the unmanned device to control the unmanned device to continue to travel along the second trajectory. If not, the first trajectory and the second trajectory are re-fused to obtain an intermediate trajectory until the obtained intermediate trajectory meets the preset condition. The fusing the first trajectory and the second trajectory to obtain the intermediate trajectory specifically includes: adjusting the first trajectory according to a preset cost function with minimizing the cost function value as an optimization target to obtain a fused trajectory; Determine a cost function value according to the first trajectory, the second trajectory, the fused trajectory and a preset cost function, wherein the cost function value represents the sum of differences between the fused trajectory and the first trajectory and the second trajectory respectively; and determine whether the cost function value is greater than a preset third threshold; If so, the first trajectory is readjusted with minimizing the cost function value as the optimization goal to obtain a fused trajectory until the cost function value is no greater than the preset third threshold or the number of fused trajectories obtained reaches a second preset number of iterations; if not, the obtained fused trajectory is used as the intermediate trajectory.

9. A computer-readable storage medium, characterized in that: The storage medium stores a computer program, and when the computer program is executed by a processor, the method described in any one of claims 1 to 7 is implemented.

10. An unmanned device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the program, the method described in any one of claims 1 to 7 is implemented.

Images