Reversing path planning method and readable storage medium

Abstract

The invention provides a backing-up path planning method and a readable storage medium. The backing-up path planning method comprises the following steps: acquiring a historical track queue, a required track length and an actual available track length of a target vehicle; according to the required track length and the actual available track length, in combination with a collision prediction result of the target vehicle, judging whether the target vehicle enters a backing tracking mode or not, and under the condition that the target vehicle enters the backing tracking mode, obtaining a backing track of the target vehicle according to the historical track queue; and / or under the condition that the target vehicle does not enter the backing tracking mode, acquiring a backing track of the target vehicle according to the speed domain and the angular speed domain based on vehicle dynamics and vehicle collision conditions. The technical problem that in the prior art, when a memorized route is shielded, the memorized reversing assist directly quits the function, and the user experience feeling is poor is solved.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning, and in particular to a reversing path planning method and a readable storage medium. Background Art

[0002] In scenarios such as parking lots or unprotected alleys, assisted driving will focus more on parking or how to drive out of the relevant scenes to a safe area, but they rarely consider how to escape when the vehicle is stuck in a "dead end" or encounters a road ahead that cannot be driven.

[0003] In relevant reversing technology solutions, more focus will be placed on memorizing reversing strategies, that is, by memorizing the routes traveled, the driver can be given corresponding reversing assistance when encountering a situation where the vehicle cannot move forward, which can especially provide greater help to novice drivers.

[0004] However, in the actual implementation process, there is such a problem: when the memorized route is blocked, the memory reversing assist cannot continue and will directly exit the function. This is poor for the user experience and is not perfect in terms of functional integrity. Summary of the Invention

[0005] The present invention solves the technical problem in the prior art that when the memorized route is blocked, the memory reversing assist function will be directly exited, resulting in a poor user experience.

[0006] To address the above-mentioned problems, the present invention provides a reversing path planning method, comprising: obtaining a historical trajectory queue, a required trajectory length, and an actually available trajectory length of a target vehicle; determining whether the target vehicle enters a reversing tracking mode based on the required trajectory length and the actually available trajectory length, in combination with a collision prediction result of the target vehicle; if the target vehicle enters the reversing tracking mode, obtaining the reversing trajectory of the target vehicle based on the historical trajectory queue; and / or if the target vehicle does not enter the reversing tracking mode, obtaining the reversing trajectory of the target vehicle based on a velocity domain and an angular velocity domain based on vehicle dynamics and vehicle collision conditions; wherein the required trajectory length is the historical trajectory length required by the target vehicle, and the actually available trajectory length is the historical trajectory length actually available to the target vehicle.

[0007] Compared with the existing technology, the technical effect achieved by adopting this technical solution is: in the existing technology, the reversing of the target vehicle will be more focused on the memory reversing strategy, that is, the driver is given corresponding reversing assistance through the memory route, which can especially provide greater help to novice drivers. At this time, the target vehicle enters the reversing tracking mode; there are also cases where the target vehicle cannot enter the reversing tracking mode, which may be due to reasons such as the memory route is too short, the memory route ends, or the memory route cannot be executed due to occlusion. The planning method in this application supports self-vehicle reversing exploration in this scenario, and can obtain the reversing trajectory of the target vehicle according to the speed domain and angular velocity domain based on vehicle dynamics and vehicle collision conditions, instead of directly exiting the reversing function.

[0008] In one example of the present invention, the collision prediction result includes: the presence of a collision and the absence of a collision; based on the required trajectory length and the actual available trajectory length, combined with the collision prediction result of the target vehicle, determining whether the target vehicle enters the reverse tracking mode, including: when the actual available trajectory length ≤ the required trajectory length, determining that the target vehicle does not enter the reverse tracking mode; and / or when the actual available trajectory length > the required trajectory length, determining whether the target vehicle enters the reverse tracking mode in combination with the collision prediction result; if the collision prediction result is that there is a collision, determining that the target vehicle does not enter the reverse tracking mode; and / or if the collision prediction result is that there is no collision, determining that the target vehicle enters the reverse tracking mode.

[0009] Compared with the existing technology, the technical effect achieved by adopting this technical solution is as follows: If the actual available trajectory length is greater than the required trajectory length, it means that the actual available historical trajectory length meets the planning requirements, and it is judged whether the target vehicle will collide in this historical trajectory. If there is a collision, it means that the target vehicle’s memorized route is blocked when performing memory reverse assist, and it is necessary to enter reverse obstacle avoidance mode. If there is no collision, the target vehicle needs to enter reverse tracking mode, and the target vehicle can perform memory reverse assist. If the actual available trajectory length is less than or equal to the required trajectory length, it means that the actual available historical trajectory length does not meet the planning requirements, and the target vehicle needs to enter reverse exploration mode.

[0010] In one embodiment of the present invention, the historical trajectory queue is ; When the target vehicle enters the reverse tracking mode, the target vehicle's reverse trajectory is obtained according to the historical trajectory queue, including: combining the most recent historical trajectory point and the historical trajectory queue to obtain the first trajectory queue as ; Obtain the target trajectory queue of the target vehicle according to the first trajectory queue to obtain the reversing trajectory; wherein, is the i-th historical trajectory point in the historical trajectory queue, is the nth historical trajectory point in the historical trajectory queue; is the most recent historical trajectory point, ≤n.

[0011] In one embodiment of the present invention, obtaining the historical trajectory queue, the required trajectory length, and the actual available trajectory length of the target vehicle includes: combining the real-time speed of the vehicle, the planning time, and the minimum historical trajectory threshold to obtain the required trajectory length as ; Match the current coordinates of the target vehicle with the historical trajectory queue to obtain the most recent historical trajectory point; calculate the actual available trajectory length based on the most recent historical trajectory point ;in, is the minimum historical trajectory threshold, is the real-time speed of the vehicle, Planning duration; for The horizontal coordinate in the world coordinate system, for The vertical coordinate in the world coordinate system; for The horizontal coordinate in the world coordinate system, for The vertical coordinate in the world coordinate system.

[0012] Compared with the existing technology, the technical effect achieved by adopting this technical solution is: matching the current coordinates with the historical trajectory queue to obtain the nearest historical trajectory point in the historical trajectory queue , delete the historical track that has been traveled, and the historical track queue becomes the first track queue ; Backward calculation ,until Greater than , the calculation formula is: , is the Euclidean distance between the i-1th historical trajectory point and the i-th historical trajectory point.

[0013] In one embodiment of the present invention, when the target vehicle does not enter the reverse tracking mode, the reverse trajectory of the target vehicle is obtained according to the speed domain and the angular velocity domain based on vehicle dynamics and vehicle collision conditions, including: obtaining the speed domain and the angular velocity domain based on scene restrictions and vehicle parameter restrictions; obtaining a trajectory set of the target vehicle according to the speed domain and the angular velocity domain based on vehicle dynamics and vehicle collision conditions; performing a cost calculation on the first trajectory in the trajectory set according to the required trajectory length and the actual available trajectory length to obtain an optimal trajectory; and controlling the target vehicle according to the optimal trajectory.

[0014] In one embodiment of the present invention, the trajectory set is: ; Based on the required trajectory length and the actual available trajectory length, a cost calculation is performed on the first trajectory in the trajectory set to obtain the optimal trajectory, including: when the actual available trajectory length is ≤ the required trajectory length, the cost calculation is performed on the first trajectory using a first cost function; and / or when the actual available trajectory length is greater than the required trajectory length, the cost calculation is performed on the first trajectory using a second cost function; wherein the first cost function is: ; The second cost function is: ; is a set of trajectories, is the jth first trajectory in the trajectory set; For the trajectory The speed cost function, For the trajectory The obstacle distance cost function is For the trajectory The angular velocity cost function is, For the trajectory The historical trajectory similarity cost function is used.

[0015] Compared with the existing technology, the technical effect achieved by adopting this technical solution is as follows: when the target vehicle does not enter the reverse tracking mode, by judging the actual available track length and the required track length, it is possible to determine why the target vehicle did not enter the reverse tracking mode: when the actual available track length is ≤ the required track length, it means that the actual available historical track length does not meet the planning requirements and the safety of this planning cannot be guaranteed. Therefore, the target vehicle cannot enter the reverse tracking mode and enters the reverse exploration mode; when the actual available track length is greater than the required track length, it means that the actual available historical track length meets the planning requirements, but if the target vehicle enters the reverse tracking mode, there will be a collision. Therefore, the target vehicle enters the reverse obstacle avoidance mode. The cost function calculation in the reverse exploration mode and the reverse obstacle avoidance mode is different. When encountering obstacles in the reverse obstacle avoidance mode, the vehicle will reasonably and effectively avoid them until it returns to the memory route and then enters the reverse tracking mode.

[0016] In one embodiment of the present invention, the speed cost function is: ; The obstacle distance cost function is: ; The angular velocity cost function is: ; The historical trajectory similarity cost function is: ;in, is the speed cost coefficient, For the trajectory At the track point speed, The maximum reversing speed under the reversing function; is the obstacle distance cost coefficient, For the trajectory At the track point distance to obstacles; is the angular velocity cost coefficient, For the trajectory At the track point angular velocity; is the historical trajectory similarity cost coefficient, For trajectory points The horizontal coordinate in the world coordinate system, For trajectory points The vertical coordinate in the world coordinate system, for Matching points in the historical trajectory, for Matching points in the historical trajectory.

[0017] Compared with the existing technology, the technical effect achieved by adopting this technical solution is:

[0018] In one embodiment of the present invention, obtaining a speed domain and an angular velocity domain based on scenario restrictions and vehicle parameter restrictions includes: obtaining a vehicle-related speed interval, an acceleration-related speed interval, and a collision-related speed interval; obtaining a vehicle-related angular velocity interval, an acceleration-related angular velocity interval, and a collision-related angular velocity interval; obtaining a speed domain based on the vehicle-related speed interval, the acceleration-related speed interval, and the collision-related speed interval; obtaining an angular velocity domain based on the vehicle-related angular velocity interval, the acceleration-related angular velocity interval, and the collision-related angular velocity interval; wherein the vehicle-related speed interval is: , the acceleration-related speed range is: , the collision-related velocity range is: ; The vehicle's relevant angular velocity range is: , the acceleration-related angular velocity range is: , the collision-related angular velocity range is: .

[0019] Compared with the existing technology, the technical effect achieved by adopting this technical solution is: based on the scene restrictions and the vehicle's own parameter restrictions, the vehicle-related speed range of the target vehicle is obtained. ; Get the acceleration-related speed range of the target vehicle based on comfort constraints Angular velocity range related to acceleration ; Get the collision-related speed range of the target vehicle based on the collision limit Collision-related angular velocity range .

[0020] In one embodiment of the present invention, the velocity domain is: ; The angular velocity domain is: ;in, is the first vehicle-related speed threshold, is the first acceleration-related speed threshold, is the first collision-related speed threshold; is the second vehicle-related speed threshold, is the second acceleration-related speed threshold, is the second collision-related speed threshold; is the first vehicle-related angular velocity threshold, is the first acceleration-related angular velocity threshold, is the first collision-related angular velocity threshold; is the second vehicle-related angular velocity threshold, is the second acceleration-related angular velocity threshold, is the second collision-related angular velocity threshold.

[0021] Compared with the existing technology, the technical effect achieved by adopting this technical solution is: after obtaining the vehicle-related speed interval, acceleration-related speed interval, collision-related speed interval and vehicle-related angular velocity interval, acceleration-related angular velocity interval, collision-related angular velocity interval, the speed domain can be obtained through the vehicle-related speed interval, acceleration-related speed interval and collision-related speed interval; the angular velocity domain can be obtained through the vehicle-related angular velocity interval, acceleration-related angular velocity interval and collision-related angular velocity interval.

[0022] On the other hand, an embodiment of the present invention further provides a readable storage medium, which stores a program or instruction. When the program or instruction is executed by a processor, the steps of the reversing path planning method as described in any of the above embodiments are implemented.

[0023] Compared with the existing technology, the technical effect achieved by adopting this technical solution is: the readable storage medium in this embodiment is used to store the reversing path planning method as in any embodiment of the present invention, so it has all the beneficial effects of the reversing path planning method as in any embodiment of the present invention, which will not be repeated here.

[0024] After adopting the technical solution of the present invention, the following technical effects can be achieved: (1) In the prior art, the reversing of the target vehicle is more focused on the memory reversing strategy, that is, the driver is given corresponding reversing assistance by memorizing the route, which can especially provide great help to novice drivers. At this time, the target vehicle enters the reversing tracking mode; there are also cases where the target vehicle cannot enter the reversing tracking mode, which may be due to reasons such as the memorized route being too short, the memorized route ending, or the memorized route being unable to be executed due to occlusion. The planning method in this application supports the self-vehicle reversing exploration in this scenario, and can obtain the reversing trajectory of the target vehicle according to the speed domain and angular velocity domain based on vehicle dynamics and vehicle collision conditions, rather than directly exiting the reversing function; (2) The cost function calculations in the reversing exploration mode and the reversing obstacle avoidance mode are different. In the reversing obstacle avoidance mode, when encountering an obstacle, the vehicle will avoid it reasonably and effectively until it returns to the memory route, and then enter the reversing tracking mode. BRIEF DESCRIPTION OF THE DRAWINGS

[0025] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the following briefly introduces the drawings to be used in describing the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. Those skilled in the art can also derive other drawings based on these drawings without inventive efforts. Figure 1 A flowchart of a reversing path planning method provided in Example 1 of the present invention; Figure 2 A schematic structural diagram of a readable storage medium provided in Embodiment 2 of the present invention.

[0026] Description of reference numerals: 100 - readable storage medium; 110 - computer executable instructions. DETAILED DESCRIPTION

[0027] To make the above-mentioned objectives, features, and advantages of the present invention more clearly understood, the technical solutions in the embodiments of the present invention are clearly and completely described. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of them. All other embodiments obtained by persons of ordinary skill in the art based on the embodiments of the present invention without creative effort shall fall within the scope of protection of the present invention.

[0028] [Example 1] See also Figure 1 , which is a flow chart of a reversing path planning method provided by the first embodiment of the present invention. The reversing path planning method includes: S100: Obtain the historical trajectory queue, required trajectory length, and actual available trajectory length of the target vehicle; S200: Determining whether the target vehicle enters a reverse tracking mode based on the required track length and the actual available track length and the collision prediction result of the target vehicle; S310: When the target vehicle enters the reverse tracking mode, obtaining the reverse track of the target vehicle according to the historical track queue; and / or S320: When the target vehicle has not entered the reverse tracking mode, obtaining a reverse trajectory of the target vehicle in the velocity domain and the angular velocity domain based on vehicle dynamics and vehicle collision conditions; The required trajectory length is the historical trajectory length required by the target vehicle, and the actual available trajectory length is the historical trajectory length actually available to the target vehicle.

[0029] In a specific embodiment, the actual available trajectory length is the actual available historical trajectory length, and the required trajectory length is the required historical trajectory length. The historical trajectory queue stores multiple historical trajectory points of the target vehicle on the historical trajectory (i.e., the memory route). In the prior art, the reversing of the target vehicle is more focused on the memory reversing strategy, that is, providing the driver with corresponding reversing assistance through the memory route, which can be particularly helpful to novice drivers. At this time, the target vehicle enters the reverse tracking mode; there are also cases where the target vehicle cannot enter the reverse tracking mode. This may be due to reasons such as the memory route being too short, the memory route ending, or the memory route being unable to be executed due to occlusion. The planning method in this application supports self-vehicle reversing exploration in this scenario. It can obtain the reversing trajectory of the target vehicle based on the speed domain and angular velocity domain based on vehicle dynamics and vehicle collision conditions, rather than directly exiting the reversing function.

[0030] Preferably, each historical track point consists of a series of information: , yes The vehicle coordinates in the world coordinate system; yes The total velocity of the vehicle in the world coordinate system; yes The total acceleration in the world coordinate system; yes Heading angle in world coordinate system; yes timestamp.

[0031] Furthermore, the collision prediction result includes: whether a collision exists or not; S200 includes: S210: When the actual available track length is less than or equal to the required track length, determining that the target vehicle has not entered the reverse tracking mode; and / or S220: When the actual available track length is greater than the required track length, judging whether the target vehicle enters the reverse tracking mode in combination with the collision prediction result; S221: If the collision prediction result indicates that a collision has occurred, determining that the target vehicle has not entered the reverse tracking mode; and / or S222: When the collision prediction result indicates that there is no collision, it is determined that the target vehicle enters a reverse tracking mode.

[0032] Specifically, if , indicating that the actual available historical trajectory length meets the planning requirements, and judging whether the target vehicle will have a collision in this historical trajectory: if there is a collision, it means that the target vehicle has a memorized route blocked when performing memory reverse assist, and it needs to enter the reverse obstacle avoidance mode; if there is no collision, the target vehicle needs to enter the reverse tracking mode, and the target vehicle can perform memory reverse assist. If , indicating that the actual available historical trajectory length does not meet the planning requirements, and the target vehicle needs to enter the reverse exploration mode. is the actual available trajectory length; is the required track length.

[0033] Furthermore, the historical trajectory queue is ; S310 includes: S311: Combine the most recent historical trajectory point and the historical trajectory queue to obtain the first trajectory queue: ; S312: Obtain a target trajectory queue of the target vehicle according to the first trajectory queue to obtain a reversing trajectory; in, is the i-th historical trajectory point in the historical trajectory queue, is the nth historical trajectory point in the historical trajectory queue; is the most recent historical trajectory point, ≤n.

[0034] Specifically, combining the most recent historical track point and the historical track queue, deleting the historical track that has been traveled to obtain the first track queue as , The most recent historical trajectory point represents the trajectory point in the historical trajectory queue that is closest to the current coordinates of the target vehicle, and also represents the egoth trajectory point in the historical trajectory queue. The historical trajectory points in the first trajectory queue constitute the memory route that the target vehicle can travel in the subsequent planning process (represented by the historical trajectory in this application).

[0035] The specific process of reverse tracking mode includes: obtaining the first track queue ; ; ; ; Deposit ; release .

[0036] In simple terms, it is to start from the most recent historical point in the recorded historical trajectory. Start taking points forward (i.e., from the back to the front); the parameters of each historical trajectory point remain consistent, and the time is recalculated from the planning moment (i.e., starting from 0); stop when the required point meets the trajectory time length.

[0037] Furthermore, S100 includes: S110: Combine the real-time speed of the vehicle, the planning time and the minimum historical trajectory threshold to obtain the required trajectory length: ; S120: Match the current coordinates of the target vehicle with the historical trajectory queue to obtain the most recent historical trajectory point; S130: Calculate the actual available track length based on the most recent historical track points ; in, is the minimum historical trajectory threshold, is the real-time speed of the vehicle, Planning duration; for The horizontal coordinate in the world coordinate system, for The vertical coordinate in the world coordinate system; for The horizontal coordinate in the world coordinate system, for The vertical coordinate in the world coordinate system.

[0038] Specifically, is the minimum historical trajectory threshold, that is, the minimum requirement for the length of the historical trajectory. It can improve the security of the planning method of this application; Planning duration is the total duration of the planned trajectory and is the default value. Specifically explained as: from the matching point Start, advance to the actual available distance in the historical trajectory of the starting point, and calculate The specific process is: match the current coordinates with the historical trajectory queue, and obtain the nearest historical trajectory point in the historical trajectory queue. , delete the historical track that has been traveled, and the historical track queue becomes the first track queue ; Backward calculation ,until Greater than , the calculation formula is: , is the Euclidean distance between the i-1th historical trajectory point and the i-th historical trajectory point. The preferred value is 20m.

[0039] For example, if we have been reversing or have just started recording tracks, the length of the historical track that can be used may be 10m, but we need at least 20m (i.e. ) can ensure the safety of this planning. Less than or equal to , it means that the actual available historical trajectory length does not meet the planning requirements.

[0040] Preferably, before step S100, the reverse path planning method further includes: obtaining a grid map of the area where the target vehicle is located; specifically, obtaining body and chassis data of the target vehicle and a grid map of the sensing end. This application requires less data, requiring only a basic sensing end to be provided for the drivable area, effectively reducing reliance on sensors and the hardware cost required for the technical application.

[0041] Furthermore, S320 includes: S321: Obtaining a velocity domain and an angular velocity domain based on scene restrictions and vehicle parameter restrictions; S322: Based on vehicle dynamics and vehicle collision conditions, a trajectory set of the target vehicle is obtained according to the velocity domain and the angular velocity domain; S323: performing cost calculation on the first trajectory in the trajectory set according to the required trajectory length and the actual available trajectory length to obtain the optimal trajectory; S324: Control the target vehicle according to the optimal trajectory.

[0042] Furthermore, the trajectory set is: ; S323 includes: S3231: When the actual available trajectory length is less than or equal to the required trajectory length, performing cost calculation on the first trajectory using a first cost function; and / or S3232: When the actual available trajectory length is greater than the required trajectory length, the cost of the first trajectory is calculated using the second cost function; Among them, the first cost function is: ; The second cost function is: ; is a set of trajectories, is the jth first trajectory in the trajectory set; For the trajectory The speed cost function, For the trajectory The obstacle distance cost function is For the trajectory The angular velocity cost function is, For the trajectory The historical trajectory similarity cost function is used.

[0043] Specifically, after obtaining the velocity domain and angular velocity domain, the trajectory set is calculated based on vehicle dynamics and vehicle collision conditions as follows: When the target vehicle does not enter reverse tracking mode, the reason for not entering reverse tracking mode can be determined by comparing the actual available track length with the required track length. When the actual available track length is less than or equal to the required track length, the actual available historical track length does not meet the planning requirements and cannot guarantee the safety of this plan. Therefore, the target vehicle cannot enter reverse tracking mode and enters reverse exploration mode. When the actual available track length is greater than the required track length, the actual available historical track length meets the planning requirements, but entering reverse tracking mode would result in a collision. Therefore, the target vehicle enters reverse obstacle avoidance mode. Steps S321 and S322 of reverse obstacle avoidance mode are the same as those of reverse exploration mode.

[0044] In the reverse exploration mode, traverse the j first trajectories in the trajectory set, and for each trajectory point of each first trajectory Calculate the cost of each first trajectory by calculating the speed cost, obstacle distance cost and angular velocity cost (represented by the first cost function in this application) for each first trajectory; in the reverse obstacle avoidance mode, traverse the j first trajectories in the trajectory set, and for each trajectory point of each first trajectory The calculation is performed, but there are some differences in the calculation of the first trajectory cost. The speed cost, obstacle distance cost, angular velocity cost and historical trajectory similarity cost function are calculated for each first trajectory to obtain the cost of each first trajectory (represented by the second cost function in this application).

[0045] Preferably, the specific steps of calculating and obtaining the trajectory set based on vehicle dynamics and vehicle collision conditions are: For v, V, dv; For w, W, dw; Vehicle dynamics recursively calculates the trajectory at the current speed and angular velocity, ; The time length is ; Perform collision calculation for each trajectory: After deleting the trajectories that may have collision risks, obtain a safe trajectory set. .

[0046] Where V is the velocity domain, v is the actual velocity value in the velocity domain, and dv is the velocity value interval; W is the angular velocity domain, w is the actual angular velocity value in the angular velocity domain, and dw is the angular velocity value interval; The length of time for the planned trajectory to be released, The preferred value is 8 seconds. Through the steps, a specific actual speed value and actual angular speed value can be obtained from a larger range (ie, speed domain or angular speed domain).

[0047] Furthermore, the speed cost function is: ; The obstacle distance cost function is: ; The angular velocity cost function is: ; The historical trajectory similarity cost function is: ; in, is the speed cost coefficient, For the trajectory At the track point speed, The maximum reversing speed under the reversing function; is the obstacle distance cost coefficient, For the trajectory At the track point distance to obstacles; is the angular velocity cost coefficient, For the trajectory At the track point angular velocity; is the historical trajectory similarity cost coefficient, For trajectory points The horizontal coordinate in the world coordinate system, For trajectory points The vertical coordinate in the world coordinate system, for Matching points in the historical trajectory, for Matching points in the historical trajectory.

[0048] Specifically, by calculating the cost of the j first trajectories, the corresponding set is obtained: ,in, is the cost of the 0th first trajectory; is the cost of the jth first trajectory. Finally, the first trajectory with the highest cost function is obtained and published to the downstream control as the optimal trajectory for vehicle control.

[0049] Preferably, ;when hour, = ;when hour, = . It is the safety distance threshold of obstacles, which is generally set to 1 meter. That is, 1 meter away is absolutely safe. 1 meter away is a constant function, and 1 meter away is a linear function.

[0050] Furthermore, S321 includes: S3211: Obtain vehicle-related speed intervals, acceleration-related speed intervals, and collision-related speed intervals; S32122: Obtain vehicle-related angular velocity intervals, acceleration-related angular velocity intervals, and collision-related angular velocity intervals; S3213: Obtain a speed domain based on the vehicle-related speed interval, the acceleration-related speed interval, and the collision-related speed interval; S3214: Obtain an angular velocity domain according to the vehicle-related angular velocity interval, the acceleration-related angular velocity interval, and the collision-related angular velocity interval; Among them, the vehicle-related speed range is: , the acceleration-related speed range is: , the collision-related velocity range is: ; The vehicle's relevant angular velocity range is: , the acceleration-related angular velocity range is: , the collision-related angular velocity range is: .

[0051] Specifically, the vehicle-related speed range of the target vehicle is obtained based on the scene restrictions and the vehicle's own parameter restrictions Due to sensor limitations, it is difficult to obtain environmental information from a longer distance in reversing scenarios. The detailed information that can be obtained is limited to a range of about 15 meters behind the vehicle. Therefore, it is necessary to constrain the reversing speed and direction: , is the first vehicle-related speed threshold, is the second vehicle-related speed threshold; preferably, ; , which is the maximum reversing speed under the reversing function. Based on the steering wheel limit of the vehicle itself, the vehicle-related angular velocity range of the target vehicle is obtained. , the steering wheel of the target vehicle itself has a limit, which can be defined as: , is the first vehicle-related angular velocity threshold, is the second vehicle-related angular velocity threshold; preferably, , which is the minimum angular velocity value related to the vehicle body; , which is the maximum angular velocity value related to the vehicle body, It is the threshold value of the vehicle body's unilateral steering limit.

[0052] Obtain the acceleration-related speed range of the target vehicle based on comfort constraints Angular velocity range related to acceleration In reverse mode, speed smoothness is constrained by acceleration, preventing sudden acceleration or deceleration. Lateral comfort is constrained by angular acceleration, which is expressed as the steering wheel rotation rate. The steering wheel should not change significantly within a unit of time, otherwise it will cause riding discomfort. This means that the large centrifugal force will prevent passengers from sitting properly, potentially causing damage to the vehicle itself and other vehicles. , is the first acceleration-related speed threshold, is the second acceleration-related speed threshold; preferably, , , is the ego vehicle speed of the target vehicle at the beginning of planning, is the difference in planning time points, is the minimum acceleration in the current scene, is the maximum acceleration in the current scene. , is the first acceleration-related angular velocity threshold, is the second acceleration-related angular velocity threshold; preferably, , , is the angular velocity of the ego vehicle at the beginning of planning; is the difference in planning time points; is the minimum angular acceleration in the current scene; is the maximum angular acceleration in the current scene.

[0053] Obtain the collision-related speed range of the target vehicle based on collision limits Collision-related angular velocity range , obtain the nearest collision distance of the target vehicle in its current state, and calculate the maximum speed at which it can be stopped while ensuring a safe distance: , is the first collision-related speed threshold, is the second collision-related speed threshold; preferably, , , is the Euclidean distance between the target vehicle and the obstacle at the current relative speed and angular velocity. , is the first collision-related angular velocity threshold, is the second collision-related angular velocity threshold; preferably, , , for is the maximum angular velocity in the current scene.

[0054] Furthermore, the velocity domain is: ; The angular velocity domain is: ; in, is the first vehicle-related speed threshold, is the first acceleration-related speed threshold, is the first collision-related speed threshold; is the second vehicle-related speed threshold, is the second acceleration-related speed threshold, is the second collision-related speed threshold; is the first vehicle-related angular velocity threshold, is the first acceleration-related angular velocity threshold, is the first collision-related angular velocity threshold; is the second vehicle-related angular velocity threshold, is the second acceleration-related angular velocity threshold, is the second collision-related angular velocity threshold.

[0055] Specifically, after obtaining the vehicle-related speed interval, acceleration-related speed interval, collision-related speed interval and vehicle-related angular velocity interval, acceleration-related angular velocity interval, collision-related angular velocity interval, the speed domain can be obtained through the vehicle-related speed interval, acceleration-related speed interval and collision-related speed interval; the angular velocity domain can be obtained through the vehicle-related angular velocity interval, acceleration-related angular velocity interval and collision-related angular velocity interval.

[0056] Among them, it can be expressed as , , is the first speed threshold, is the second speed threshold; , , is the first angular velocity threshold, is the second angular velocity threshold. , V is the velocity domain, that is, the final velocity range; , W is the angular velocity domain, that is, the final angular velocity range.

[0057] Preferably, , is the number of speed samples, The preferred value of is 7; is the number of angular velocity samples. The value of v is: , the value of w is: .

[0058] [Example 2] See also Figure 2 This embodiment also provides a readable storage medium 100, which stores computer-executable instructions 110. When the computer-executable instructions 110 are read and executed by the processor, the target vehicle where the readable storage medium 100 is located is controlled to implement the reversing path planning method in the first embodiment.

[0059] In the several embodiments provided in this application, it should be understood that the disclosed devices and methods can also be implemented in other ways. The device embodiments described above are merely illustrative. For example, the functional modules in the various embodiments of the present invention can be integrated together to form an independent part, or each module can exist independently, or two or more modules can be integrated to form an independent part.

[0060] If the function is implemented in the form of a software function module and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, or the part of the technical solution, can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes a number of instructions for enabling a computer device (which can be a personal computer, server, or network device, etc.) to execute all or part of the steps of the various embodiments of the present invention. The aforementioned readable storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk, and other media that can store program code.

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

Claims

1. A reversing path planning method, characterized in that: The reversing path planning method comprises: Obtain the historical trajectory queue, required trajectory length, and actual available trajectory length of the target vehicle; Determining whether the target vehicle enters a reverse tracking mode based on the required track length and the actual available track length and in combination with a collision prediction result of the target vehicle; When the target vehicle enters the reverse tracking mode, obtaining the reverse trajectory of the target vehicle according to the historical trajectory queue; and / or When the target vehicle does not enter the reverse tracking mode, obtaining a reverse trajectory of the target vehicle according to a velocity domain and an angular velocity domain based on vehicle dynamics and vehicle collision conditions; The required trajectory length is the historical trajectory length required by the target vehicle; and the actually available trajectory length is the actually available historical trajectory length of the target vehicle.

2. The reverse path planning method according to claim 1, characterized in that: The collision prediction results include: collision exists and collision does not exist; The determining, based on the required track length and the actual available track length and in combination with the collision prediction result of the target vehicle, whether the target vehicle enters the reverse tracking mode includes: When the actual available track length is less than or equal to the required track length, determining that the target vehicle has not entered the reverse tracking mode; and / or When the actual available trajectory length is greater than the required trajectory length, determining whether the target vehicle enters the reverse tracking mode in combination with the collision prediction result; When the collision prediction result indicates that a collision exists, determining that the target vehicle has not entered the reverse tracking mode; and / or When the collision prediction result is that there is no collision, it is determined that the target vehicle enters the reverse tracking mode.

3. The reverse path planning method according to claim 1, characterized in that: The historical trajectory queue is ; When the target vehicle enters the reverse tracking mode, obtaining the reverse trajectory of the target vehicle according to the historical trajectory queue includes: Combine the most recent historical trajectory point and the historical trajectory queue to obtain the first trajectory queue: ; Obtaining a target trajectory queue of the target vehicle according to the first trajectory queue to obtain the reversing trajectory; in, is the i-th historical trajectory point in the historical trajectory queue, is the nth historical trajectory point in the historical trajectory queue; is the most recent historical trajectory point, ≤n.

4. The reverse path planning method according to claim 3, characterized in that: The step of obtaining the historical trajectory queue, the required trajectory length, and the actual available trajectory length of the target vehicle includes: Combining the real-time speed of the vehicle, the planning time and the minimum historical trajectory threshold, the required trajectory length is obtained as ; Matching the current coordinates of the target vehicle with the historical trajectory queue to obtain the most recent historical trajectory point; Calculate the actual available trajectory length by combining the most recent historical trajectory points ; in, is the minimum historical trajectory threshold, is the real-time speed of the vehicle, Planning duration; for The horizontal coordinate in the world coordinate system, for The vertical coordinate in the world coordinate system; for The horizontal coordinate in the world coordinate system, for The vertical coordinate in the world coordinate system.

5. The reverse path planning method according to claim 1, characterized in that: When the target vehicle does not enter the reverse tracking mode, obtaining the reverse trajectory of the target vehicle according to the speed domain and the angular velocity domain based on vehicle dynamics and vehicle collision conditions includes: Acquire the velocity domain and the angular velocity domain based on scene restrictions and vehicle parameter restrictions; Based on vehicle dynamics and vehicle collision conditions, obtaining a trajectory set of the target vehicle according to the velocity domain and the angular velocity domain; performing a cost calculation on a first trajectory in the trajectory set according to the required trajectory length and the actual available trajectory length to obtain an optimal trajectory; The target vehicle is controlled according to the optimal trajectory.

6. The reverse path planning method according to claim 5, characterized in that: The trajectory set is: ; The step of performing cost calculation on a first trajectory in the trajectory set according to the required trajectory length and the actual available trajectory length to obtain an optimal trajectory includes: When the actual available trajectory length is less than or equal to the required trajectory length, performing cost calculation on the first trajectory using a first cost function; and / or When the actual available trajectory length is greater than the required trajectory length, performing cost calculation on the first trajectory using a second cost function; Wherein, the first cost function is: ; The second cost function is: ; is the trajectory set, is the jth first trajectory in the trajectory set; For the trajectory The speed cost function, For the trajectory The obstacle distance cost function is For the trajectory The angular velocity cost function is, For the trajectory The historical trajectory similarity cost function is used.

7. The reverse path planning method according to claim 6, characterized in that: The speed cost function is: ; The obstacle distance cost function is: ; The angular velocity cost function is: ; The historical trajectory similarity cost function is: ; in, is the speed cost coefficient, For the trajectory At the track point speed, The maximum reversing speed under the reversing function; is the obstacle distance cost coefficient, For the trajectory At the trajectory point distance to obstacles; is the angular velocity cost coefficient, For the trajectory At the trajectory point angular velocity; is the historical trajectory similarity cost coefficient, The trajectory point The horizontal coordinate in the world coordinate system, The trajectory point The vertical coordinate in the world coordinate system, for Matching points in the historical trajectory, for Matching points in the historical trajectory.

8. The reverse path planning method according to claim 5, characterized in that: The obtaining of the velocity domain and the angular velocity domain based on the scenario restriction and the vehicle's own parameter restriction includes: Obtain vehicle-related speed intervals, acceleration-related speed intervals, and collision-related speed intervals; Obtain vehicle-related angular velocity intervals, acceleration-related angular velocity intervals, and collision-related angular velocity intervals; Obtaining the speed domain according to the vehicle-related speed interval, the acceleration-related speed interval, and the collision-related speed interval; Obtaining the angular velocity domain according to the vehicle-related angular velocity interval, the acceleration-related angular velocity interval, and the collision-related angular velocity interval; The vehicle-related speed range is: , the acceleration-related speed range is: , the collision-related speed range is: ; The vehicle-related angular velocity range is: , the acceleration-related angular velocity range is: , the collision-related angular velocity range is: .

9. The reverse path planning method according to claim 8, characterized in that: The speed domain is: ; The angular velocity domain is: ; in, is the first vehicle-related speed threshold, is the first acceleration-related speed threshold, is the first collision-related speed threshold; is the second vehicle-related speed threshold, is the second acceleration-related speed threshold, is the second collision-related speed threshold; is the first vehicle-related angular velocity threshold, is the first acceleration-related angular velocity threshold, is the first collision-related angular velocity threshold; is the second vehicle-related angular velocity threshold, is the second acceleration-related angular velocity threshold, is the second collision-related angular velocity threshold.

10. A readable storage medium, characterized in that: The readable storage medium stores a program or instruction, and when the program or instruction is executed by the processor, the steps of the reversing path planning method according to any one of claims 1 to 9 are implemented.

Images