Prediction updating planning method for airport scene vehicle formation following task

Information is obtained through vehicle-mounted sensors and radar, combined with RRT* and B-spline smoothing planning, coordinated obstacle avoidance path prediction update and formation transformation of unmanned vehicle formations are realized, solving the problems of formation obstacle avoidance and formation transformation, and realizing the Safe and efficient follow-up in complex environments.

CN118915768AActive Publication Date: 2024-11-08BEIHANG UNIV

Patent Information

Application Number
CN202411078280.8
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-08-07
Publication Date
2024-11-08
Estimated Expiration
2044-08-07

AI Technical Summary

Technical Problem

The existing unmanned vehicle fleet follow-up and obstacle avoidance methods have not effectively solved the problem of obstacle avoidance, especially the lack of planning and control methods on the different obstacle avoidance problems brought about by formation and formation.

Method used

A formation following strategy combining planning and control is proposed, and information is obtained through vehicle-mounted sensors and radars, and a vehicle kinematic model is constructed. RRT* and B-spline smoothing planning algorithms are used to realize the prediction and update of coordinated obstacle avoidance paths, and in the formation Under the transformation strategy, we deal with narrow road scenarios and avoid collisions between vehicles through vehicle-car communication.

Benefits of technology

The vehicle formation's autonomous obstacle avoidance ability in complex environments has been achieved, the formation's known insufficient information has been overcome, a safe and efficient automatic response strategy has been provided, and the effectiveness of the method has been verified through actual vehicle experiments.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118915768A_ABST
    Figure CN118915768A_ABST
Patent Text Reader

Abstract

The invention relates to a prediction updating planning method for an airport scene vehicle formation following task, belongs to the technical field of path planning of unmanned vehicle formation, and solves the problem of different obstacle avoidance caused by formation patterns in the prior art. Acquiring information of a leading vehicle and a following vehicle and information of a road and an obstacle ahead; step S1, acquiring a local following path of a following vehicle; s2, a vehicle kinematic model is constructed, path planning and updating are carried out, and a cooperative obstacle avoidance path of the following vehicle is obtained; s3, obtaining a way-giving strategy based on the local following path of the following vehicle and the predicted path; s4, executing a formation transformation strategy of the vehicle formation, and obtaining a collision-free path of the following vehicle; s5, control input of the following vehicle is obtained according to linear model predictive control; and S6, transmitting the obtained control input of the following vehicle to the following vehicle, and controlling the following driving of the following vehicle.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the technical field of path planning for unmanned vehicle formations, and in particular to a prediction and updating planning method for a vehicle formation following task at an airport. Background Art

[0002] Autonomous vehicles are receiving extensive attention and research due to their potential to reduce vehicle spacing, thereby improving traffic throughput, reducing energy consumption, and increasing traffic safety and stability.

[0003] The vehicle formation at the airport is usually composed of multiple unmanned vehicles with independent perception, planning, and control. In the airport scene, in order to ensure the safe and smooth driving of the vehicle formation and ensure that the formation vehicles can complete the task efficiently and safely in a complex environment, a vehicle formation following planning and control method is needed, which can enable the unmanned following vehicles to perform the formation driving task under the guidance of the manned leading vehicle, and the following vehicles also have independent autonomous obstacle avoidance capabilities and can switch between passive following and active obstacle avoidance.

[0004] However, the unmanned vehicle formation following and obstacle avoidance methods currently provided in the art do not consider solving the problem of formation obstacle avoidance, nor do they solve the different obstacle avoidance problems caused by the formation formation, nor do they provide any planning and control methods for formation obstacle avoidance. Summary of the invention

[0005] In order to overcome the shortcomings of the prior art, the embodiments of the present invention provide a formation following strategy that combines planning and control. Since the task scenario targeted by the present invention is based on a formation following task, the expected planned terminal position of the following vehicle often depends on its distance from the manned leading vehicle, and the information within this distance is used as the known information of the formation. Different from the existing methods, the method provided by the embodiments of the present invention takes into account the situation where the known information of the formation is insufficient under the formation following framework, that is, in the forward direction of the formation, the distance between the leading vehicle and the following vehicle is short, and based on this, a prediction update planning method is proposed. In addition, according to the actual application scenario, the embodiments of the present invention take the triangle formation as an example, and consider two obstacle avoidance scenarios, namely, the collaborative formation obstacle avoidance under small obstacles and the formation change obstacle avoidance strategy under narrow road scenarios, and provide a clear obstacle avoidance strategy and implementation method.

[0006] According to one embodiment of the present invention, a prediction update planning method for an airport surface vehicle formation following task is provided, comprising the following steps:

[0007] Step S0, the vehicle formation is a non-in-line formation and includes a leading vehicle and a following vehicle, and the information of the leading vehicle and the following vehicle is obtained through the vehicle-mounted sensor to obtain the known path of the leading vehicle, and the information of the road ahead and obstacles is obtained through the vehicle-mounted radar;

[0008] Step S1, performing path transformation based on the known path of the leading vehicle obtained by the vehicle-mounted sensor, obtaining a local following path of the following vehicle, and making a judgment based on the information obtained by the vehicle-mounted radar. When it is judged that there is no obstacle in the forward direction of the following vehicle, the local following path of the following vehicle is output as a collision-free path and step S5 is executed. When it is judged that there is an obstacle in the forward direction of the following vehicle, step S2 is executed.

[0009] Step S2, based on the information of the leading vehicle and the following vehicle acquired by the on-board sensor, a vehicle kinematic model is constructed, path planning and updating are performed, a collaborative obstacle avoidance path of the following vehicle is obtained, and a judgment is made based on the information of obstacles acquired by the on-board radar. When it is judged that the collaborative obstacle avoidance path can enable the following vehicle to avoid obstacles safely, the collaborative obstacle avoidance path is output as a collision-free path and step S5 is executed. When it is judged that the collaborative obstacle avoidance path cannot enable the following vehicle to avoid obstacles safely, step S3 is executed.

[0010] Step S3, when it is determined according to the information obtained by the vehicle-mounted radar that the width of the road ahead of the following vehicle is not wide enough for the following vehicle in parallel to pass, a way-giving strategy is obtained based on the local following path and the predicted path of the following vehicle, and a judgment is made according to the road ahead information obtained by the vehicle-mounted radar. When it is determined that the way-giving strategy can allow the following vehicle to safely pass the road ahead, the way-giving strategy is output as a collision-free path and step S5 is executed. When it is determined that the way-giving strategy cannot allow the following vehicle to safely pass the road ahead, step S4 is executed.

[0011] Step S4, executing a formation change strategy of the vehicle formation according to the information of the leading vehicle and the following vehicle obtained by the vehicle-mounted sensor, and obtaining a collision-free path for the following vehicle;

[0012] Step S5, obtaining a control input of the following vehicle according to a linear model predictive control based on the obtained collision-free path of the following vehicle;

[0013] Step S6, transmitting the obtained control input of the following vehicle to the following vehicle, controlling the following vehicle to follow the vehicle, thereby realizing the prediction update planning of the vehicle formation following task.

[0014] In another optional embodiment, in step S0, the information of the leading vehicle and the following vehicle acquired by the on-board sensor includes: the known path, current position, speed, heading angle, and front wheel deviation angle of the leading vehicle, and the current position, heading angle, speed, and front wheel deviation angle of the following vehicle; the information of the road ahead and obstacles acquired by the on-board radar includes: the width of the road ahead, the position of the obstacle, and the shape and size of the obstacle.

[0015] In another optional implementation, step S2 specifically includes the following steps:

[0016] Step S2.1, constructing a vehicle kinematic model of the leading vehicle based on the information of the vehicle formation obtained by the on-board sensor;

[0017] Step S2.2, obtaining the predicted position of the leading vehicle based on the vehicle kinematic model of the leading vehicle, and the longitudinal path length of the predicted path is expressed as:

[0018]

[0019] Among them, d s It is the length of the path in the forward direction that the following vehicle can safely avoid obstacles, which is set based on the obstacle information obtained by the vehicle-mounted radar. h is the path length of the known path of the leading vehicle, is the ceiling function.

[0020] Step S2.3, based on the predicted position of the leading vehicle, the predicted position of the following vehicle is obtained by translation, and then the current position of the following vehicle obtained by the on-board sensor and the predicted position of the following vehicle obtained by translation are used as the starting point and the end point of the prediction planning respectively, and RRT* and B-spline smoothing planning are performed to obtain the predicted path of the following vehicle as the collaborative obstacle avoidance path;

[0021] Step S2.4, whenever the following vehicle travels along the predicted path of the following vehicle through a length d h When the path is d, the predicted position of the following vehicle will be updated, and the following vehicle will travel through a path of length d. h The position after the path and the updated predicted position are used as the starting point and end point of another prediction planning, and RRT* and B-spline smoothing planning are performed to obtain the updated predicted path of the following vehicle as the collaborative obstacle avoidance path.

[0022] Among them, the predicted path length of each updated following vehicle is:

[0023]

[0024] Repeat the prediction update process until the update number When , stop the prediction operation.

[0025] Next, the current position of the leading vehicle is directly translated to obtain the updated future position of the following vehicle. The future position of the following vehicle obtained by translation from the current position of the following vehicle is used as the starting point and end point of the prediction planning. RRT* and B-spline smoothing planning are performed to obtain the collision avoidance path of the following vehicle as the collaborative obstacle avoidance path. When d h ≥ 0.5 days s When , it is considered that the known information of the vehicle formation is sufficient;

[0026] Step S2.5, make a judgment based on the obstacle information obtained by the vehicle-mounted radar. When it is judged that the collaborative obstacle avoidance path obtained in the above steps can allow the following vehicle to avoid obstacles safely, output the collaborative obstacle avoidance path as a collision-free path and execute step S5. When it is judged that the collaborative obstacle avoidance path cannot allow the following vehicle to avoid obstacles safely, execute step S3.

[0027] In another optional implementation, step S3 specifically includes the following steps:

[0028] Step S3.1, local following paths or cooperative obstacle avoidance paths between following vehicles arranged side by side in the vehicle formation through vehicle-to-vehicle communication interaction;

[0029] Step S3.2, determining whether a local following path of a following vehicle and a cooperative obstacle avoidance path of another following vehicle collide. If it is determined that no collision occurs, the local following path and the cooperative obstacle avoidance path are used as non-collision paths and step S5 is executed. If it is determined that a collision occurs, the path obstacle avoidance planning of step S2 is repeated for the following vehicle to obtain the cooperative obstacle avoidance path of the following vehicle, wherein the cooperative obstacle avoidance path of the other following vehicle is used as the obstacle information in the prediction update during the obstacle avoidance planning process.

[0030] Step S3.3, obtaining respective cooperative obstacle avoidance paths of the two following vehicles as a way-making strategy;

[0031] Step S3.4, make a judgment based on the road ahead information obtained by the vehicle-mounted radar. When it is judged that the give way strategy can allow the following vehicle to pass the road ahead safely, output the give way strategy as a collision-free path and execute step S5; when it is judged that the give way strategy cannot allow the following vehicle to pass the road ahead safely, execute step S4.

[0032] In another optional implementation, step S4 specifically includes the following steps:

[0033] Step S4.1, transforming the vehicle formation from a non-in-line formation to an in-line formation, thereby including a middle following vehicle and a rear following vehicle;

[0034] Step S4.2, constructing a kinematic model of the vehicle and performing prediction, obtaining a predicted planning end point in the formation transformation of the following vehicles in the straight line formation, and taking the current position of the following vehicle as the prediction planning start point, based on the prediction planning start point and the prediction planning end point, using RRT* and B-spline smoothing prediction to estimate the predicted position of the leading vehicle at the entrance of the road ahead, and using the predicted position as the formation transformation end point of the middle following vehicle in the straight line formation, and the formation transformation end point of the last following vehicle in the straight line formation is obtained by translation according to the distance between the formation transformation end point of the middle following vehicle and the following vehicle;

[0035] Step S4.3, judging the known formation information of the vehicle formation, if the known formation information of the vehicle formation is sufficient, obtaining the collision-free path of the following vehicle by using the B-spline smoothing method from the current position of the following vehicle as the starting point to the formation transformation end point and executing step S4.5, if the known formation information of the vehicle formation is insufficient, executing the updating step of S4.4;

[0036] Step S4.4, the predicted planned end point of the following vehicle is updated to obtain the collision-free path of the following vehicle. When the leading vehicle passes the predicted position in S4.2, the update operation is performed, where the width of the front road as a narrow road is s∈[2d l ,4d l ], by selecting the distance between the front and rear adjacent vehicles in the straight formation, so that the longitudinal update distance is greater than d h , the expected value of the horizontal update distance is

[0037] E[u a ]=(s-2d l ) / 3

[0038] Among them, d l To maintain a safe lateral distance between the longitudinal centre axis of the following vehicle and the edge of the road ahead;

[0039] Step S4.5, after passing through the narrow road, the vehicle formation is inversely transformed, and the straight-line formation is converted back to a non-straight-line formation.

[0040] In another optional implementation, step S5 specifically includes the following steps:

[0041] Step S5.1, based on the obtained collision-free path of the following vehicle and the information of the following vehicle obtained by the on-board sensor, a discrete linear error model of the following vehicle is constructed, the obtained collision-free path of the following vehicle is used as a reference trajectory of the following vehicle, and the vehicle kinematic model of the following vehicle is linearized near the reference trajectory of the following vehicle to obtain a discrete linear error model:

[0042]

[0043] in, A k , B k , C k Related to the reference trajectory state quantity, it is a time-varying matrix, Indicates the following vehicle error state, represents the following vehicle error input, represents the following vehicle error output, x i (k) represents the position of the following vehicle in the x direction, x f (k) represents the position of the reference point in the x direction, y i (k) represents the position of the following vehicle in the y direction, y f (k) represents the position of the reference point in the y direction, is the reference heading angle, v i (k) is the speed of the following vehicle, v f (k) is the reference speed, δ i (k) is the front wheel deflection angle of the following vehicle, δ f (k) is the reference turning angle. The state information of the following vehicle is obtained through the vehicle-mounted sensor. The reference point is the coordinate point on the reference path of the following vehicle that is closest to the current position of the following vehicle.

[0044] Step S5.2, construct a linear model predictive controller of the reference trajectory of the following vehicle, and solve the linear model predictive controller to obtain the control input of the following vehicle. The linear model predictive controller is expressed as:

[0045]

[0046] st

[0047]

[0048]

[0049]

[0050]

[0051] Among them, Np represents the prediction time domain, and Is a positive definite matrix, representing the penalty weights of error output and input; and is a vector consisting of the minimum and maximum values ​​of the speed and front wheel deflection angle of the following vehicle; X i (k) is the state of the following vehicle, X f(k) is the reference state, To predict the output of t steps at time k, is the prediction input for t steps forward at time k, is the prediction error state of t steps predicted at time k; is a symmetric positive definite matrix used to determine the parameters of the obstacle ellipse. represents the distance between the following vehicle and the obstacle on the x-axis and y-axis, and

[0052]

[0053] Among them, P o is the position of the obstacle, the constraint Ensure that the following vehicle stays away from the vicinity of the obstacle represented by the ellipse,

[0054] At time k in each sampling cycle, solve the above optimization problem and obtain the optimal control input sequence Then the control input of the following vehicle is

[0055]

[0056] Among them, U f (k) represents the reference input, Represents the optimal error input at time k.

[0057] According to another embodiment of the present invention, a predictive update planning method for an airport vehicle formation following task is provided, which combines the planning layer and the control layer methods and considers two different formation scenarios. The method specifically includes the following steps.

[0058] In the first step, the planning layer performs local path planning and optimization to obtain the local following path of the following vehicle: based on the known environmental information and the starting and ending points of the vehicle formation's driving path, a path planning method based on RRT* (rapidly expanding random tree stars) and an optimization method based on B-spline smoothing are designed to obtain a smooth collision-free path as the basis for the overall obstacle avoidance strategy.

[0059] The second step is the planning layer’s formation collaborative obstacle avoidance based on prediction updates: In order to achieve the autonomous obstacle avoidance capability of the unmanned vehicle formation in complex environments and considering the situation where the known information of the formation may be insufficient, a planning strategy based on prediction updates is proposed. Combined with the local path planning and optimization method in the first step, the collaborative obstacle avoidance path of the following vehicles in the vehicle formation is generated as a collision-free path to achieve autonomous obstacle avoidance of the unmanned vehicle formation. In addition, the coordination between the following vehicles in the formation is also considered in the planning process.

[0060] In the third step, the planning layer transforms the formation based on predictions: For narrow road scenarios with insufficient passable area, a formation transformation strategy is provided, and a prediction strategy similar to the second step is proposed to deal with situations where the known information of the formation may be insufficient, and the formation transformation obstacle avoidance path of the following vehicles is obtained as a collision-free path to pass through the narrow road.

[0061] In the fourth step, the control layer performs a path tracking algorithm: based on the collision-free path output by the planning layer, a linear model predictive controller that takes obstacles into consideration is used to implement the planning of the formation obstacle avoidance task.

[0062] In the fifth step, the planning results obtained in the fourth step are input into the following vehicle, and the driving path of the following vehicle in the formation is tracked and controlled, thereby realizing the predictive update planning of the vehicle formation following task.

[0063] The second step of formation collaborative obstacle avoidance based on prediction update may specifically include the following steps.

[0064] By introducing the above-mentioned RRT* planning and B-spline smoothing algorithm, a smooth collision-free path is planned based on the starting point and the target point when obstacles are known. However, this planning algorithm cannot be directly applied to the airport field vehicle formation following task according to this embodiment of the present invention, which has the following two problems: (1) It is necessary to consider the situation where the known information of the vehicle formation may be insufficient. (2) For the obstacle avoidance task at the formation level, it is necessary to consider avoiding collisions between following vehicles.

[0065] In response to problem (1), before implementing the RRT* planning strategy, a prediction step is added. The future path of the leading vehicle is predicted through the vehicle kinematic model, and then the predicted planning end point of the following vehicle is obtained through path transformation. Therefore, the planning strategy in the first step can be implemented between the current position of the following vehicle and the predicted planning end point to obtain a collision-free path. On this basis, every time the following vehicle travels a certain distance (it is assumed that the known information of the formation has been fully updated), a new planning end point is obtained again through prediction, and RRT* and B-spline planning are implemented. This process is called path end point update, as shown in Figure 2 shown.

[0066] In response to problem (2), this embodiment of the present invention adopts a vehicle-to-vehicle communication method to enable the two following vehicles in the triangular formation to exchange their local paths for a period of time in the future. This local path is obtained by the obstacle avoidance planning algorithm (RRT* and B-spline planning) when the vehicle formation approaches an obstacle, and is obtained by path transformation from the known information of the formation when the vehicle formation is traveling. When one of the following vehicles performs an obstacle avoidance operation, the local path of the other following vehicle will be regarded as an impassable area, thus avoiding collisions between the following vehicles. In addition, for a following vehicle traveling in formation, if its local path collides with the local path of another following vehicle, the other following vehicle is also required to perform path planning to achieve a "give way" operation. When the two following vehicles cannot obtain a feasible path through path planning separately, a formation transformation strategy needs to be adopted.

[0067] The third step, the formation change based on prediction specifically includes the following steps.

[0068] In this step, attention is paid to the unmanned vehicle formation driving in the "narrow road" scenario. When the formation is a triangle or other non-vertical straight line form, when the formation coordination strategy in the second step cannot achieve collision avoidance, it is necessary to transform the triangular formation into a straight line formation. The overall formation transformation path can be directly obtained by the B-spline smoothing method from the starting point to the end point. However, in this scenario, it is still necessary to consider the problem that the known information of the formation may be insufficient due to the vehicle formation following task. In this scenario, a prediction based on RRT* and B-spline is used to estimate the position of the leading vehicle at the narrow road intersection, and this is used as the formation transformation end point of the middle vehicle in the straight line formation, and the end point of the other following vehicle in the straight line formation can also be directly obtained from the straight line formation configuration, such as Figure 3 As shown in the figure. Due to the narrow passable area, the actual passing position of the leading vehicle has a small deviation from the predicted end point. When the actual position of the following vehicle is a certain distance away from the predicted end point, a position update is performed to overcome this small deviation and consider the lane change behavior of the leading vehicle during the formation change.

[0069] The fourth step of the path tracking algorithm specifically includes the following steps.

[0070] In this step, the vehicle model is linearized and a linear model predictive controller is used to implement the path tracking task. In the construction of the linear model predictive controller, the position constraints brought by obstacles are considered to achieve local modification of the path near the obstacles.

[0071] The predictive update planning method for the airport surface vehicle formation following task provided according to the embodiment of the present invention includes at least the following advantages compared with the prior art.

[0072] 1. A platoon obstacle avoidance driving framework combining planning and control is constructed for airport field following scenarios to achieve vehicle platoon following planning and control for more diverse and uncertain scenarios.

[0073] 2. According to the needs of actual obstacle avoidance scenarios, the implementation methods of the present invention provide formation collaborative obstacle avoidance and formation change obstacle avoidance strategies, thereby providing a safe and efficient automatic response strategy for vehicle formations in more complex and changeable scenarios, and have been fully verified through actual vehicle experiments.

[0074] 3. Based on the characteristics of the following task, a prediction update strategy is provided to overcome the problem that the known information of the formation may be insufficient, and at the same time enable the vehicle formation to overcome the unexpected behavior of the manned leader vehicle during obstacle avoidance (for example, lane change, etc.). In addition, this strategy increases the applicability and scalability of this formation obstacle avoidance driving framework. BRIEF DESCRIPTION OF THE DRAWINGS

[0075] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments will be briefly introduced below. The features and advantages of the present invention can be more clearly understood by referring to the drawings. The drawings are schematic and should not be understood as limiting the present invention in any way. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0076] Figure 1 A schematic diagram of a vehicle formation following obstacle avoidance framework in a predictive update planning method for an airport surface vehicle formation following task provided according to an embodiment of the present invention.

[0077] Figure 2 A schematic diagram of collaborative obstacle avoidance of a formation based on updating in a predictive update planning method for an airport surface vehicle formation following task provided according to an embodiment of the present invention.

[0078] Figure 3 A schematic diagram of path planning for a give-way strategy in a predictive update planning method for an airport surface vehicle formation following task provided according to an embodiment of the present invention.

[0079] Figure 4 A schematic diagram of vehicle formation transformation based on prediction in a prediction update planning method for an airport vehicle formation following task provided according to an embodiment of the present invention.

[0080] Figure 5 A schematic diagram of the decomposition of a vehicle formation in a V-shaped formation is provided for applying the prediction update planning method for the airport surface vehicle formation following task provided in accordance with an embodiment of the present invention to a vehicle formation.

[0081] Figure 6A schematic diagram of an obstacle avoidance route for a vehicle formation in a diamond formation when the prediction update planning method for an airport surface vehicle formation following task provided in accordance with an embodiment of the present invention is applied.

[0082] Figure 7 A schematic diagram of an obstacle avoidance route for a vehicle formation in a V-shaped formation when the prediction update planning method for an airport surface vehicle formation following task provided in accordance with an embodiment of the present invention is applied.

[0083] Figure 8 A schematic diagram of a cooperative obstacle avoidance driving path for a vehicle formation in a triangular formation by applying the prediction update planning method for an airport surface vehicle formation following task provided in accordance with an embodiment of the present invention.

[0084] Fig. 9 The diagram is a schematic diagram of applying the predictive update planning method for the airport surface vehicle formation following task provided by an embodiment of the present invention to the updated planning path of a vehicle formation in a triangular formation.

[0085] Fig.10 A schematic diagram of vehicle speed curves of a vehicle formation in a triangular formation when the prediction update planning method for an airport surface vehicle formation following task provided in accordance with an embodiment of the present invention is applied.

[0086] Fig.11 A schematic diagram of an obstacle avoidance driving path for applying the prediction update planning method for an airport vehicle formation following task provided in accordance with an embodiment of the present invention to the formation transformation of a triangular formation through a narrow road.

[0087] Fig.12 The present invention is a schematic diagram of applying the predictive update planning method for the airport surface vehicle formation following task provided by an embodiment of the present invention to the updated planned path of a triangular formation passing through a narrow road. DETAILED DESCRIPTION

[0088] In order to more clearly understand the above-mentioned purpose, features and advantages of the present invention, the present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments. It should be noted that the embodiments of the present invention and the features in the embodiments can be combined with each other without conflict.

[0089] In the following description, many specific details are set forth to facilitate a full understanding of the present invention, but the present invention may also be implemented in other ways different from those described herein. Therefore, the protection scope of the present invention is not limited to the specific embodiments disclosed below.

[0090] The planning of unmanned vehicles can include three main levels: perception layer, planning layer and control layer. The perception layer obtains the real-time status information of the vehicle, such as vehicle position and speed, through the on-board sensors, and uses the on-board radar to obtain information about the surrounding environment and obstacles, providing basic support for subsequent path planning and control decisions. After obtaining information such as the vehicle position and obstacle position, the unmanned vehicle can realize path planning through the planning layer. Generally speaking, path planning can be divided into global path planning and local path planning. Global path planning plans a feasible trajectory from the starting point to the end point based on the global map information, while local path planning is adjusted based on the global path to achieve local obstacle avoidance function. In addition, the control layer receives the collision-free path, solves the real-time control quantity, and transmits it to the vehicle's underlying input system to ensure that the vehicle travels according to the path designed by the planning layer.

[0091] Figure 1 A schematic diagram of a vehicle formation following obstacle avoidance framework in a predictive update planning method for an airport surface vehicle formation following task provided according to an embodiment of the present invention. Figure 2 A schematic diagram of collaborative obstacle avoidance of a formation based on updating in a predictive update planning method for an airport surface vehicle formation following task provided according to an embodiment of the present invention. Figure 3 A schematic diagram of path planning for a give-way strategy in a predictive update planning method for an airport surface vehicle formation following task provided according to an embodiment of the present invention. Figure 4 The following is a schematic diagram of the vehicle formation transformation based on prediction in the prediction update planning method for the airport surface vehicle formation following task provided according to the embodiment of the present invention. The following is a detailed description of the prediction update planning method for the airport surface vehicle formation following task provided according to the embodiment of the present invention with reference to the accompanying drawings.

[0092] According to an embodiment of the present invention, a framework of a prediction update planning method for an airport surface vehicle formation following task is provided. Figure 1 The prediction update planning method is used to plan the path of each unmanned following vehicle in the vehicle formation, and the leading vehicle in the vehicle formation can be manned. The prediction update planning method for the airport field vehicle formation following task provided by the embodiment of the present invention specifically includes the following steps.

[0093] Step S0, the vehicle formation is a non-in-line formation and includes a leading vehicle and a following vehicle. The information of the leading vehicle and the following vehicle is obtained through the on-board sensor to obtain the known path of the leading vehicle, and the information of the road ahead and obstacles is obtained through the on-board radar. The information of the vehicle formation obtained by the on-board sensor includes the real-time status information of the leading vehicle and the following vehicle, such as the known path, current position, speed, heading angle, front wheel deviation angle of the leading vehicle, and the current position, heading angle, speed, front wheel deviation angle, etc. of the following vehicle. The information of the road ahead and obstacles obtained by the on-board radar includes, for example, the width of the road ahead, the position of the obstacle, the shape and size of the obstacle, etc. Figure 2 and Figure 4 In the embodiment, a triangular formation of vehicles is shown as an example of a non-in-line formation. It should be understood that the method provided in this embodiment can also be applied to other forms of non-in-line formations, such as a diamond formation, a V formation, etc. The distance between the obstacle and the current position of the following vehicle and the distance between the following vehicles can be calculated by the obtained obstacle position and the current position of each following vehicle.

[0094] Step S1, based on the known path of the leading vehicle obtained by the vehicle-mounted sensor, the path transformation is performed to obtain the local following path of the following vehicle; and according to the information obtained by the vehicle-mounted radar, a judgment is made. When it is judged that there is no obstacle in the direction of the following vehicle, the local following path of the following vehicle is output as a collision-free path and step S5 is executed; when it is judged that there is an obstacle in the direction of the following vehicle, step S2 is executed.

[0095] In this step, the path planning of the field vehicle formation in the case of straight road driving is considered, which may include the situation where the manned leading vehicle performs lane changes and other operations. In this step, the path transformation can be performed, that is, the known path that the leading vehicle has traveled at the historical moment is translated as the local following path of the following vehicle. This known path of the leading vehicle at the historical moment is also called the formation known information.

[0096] Step S2, based on the information of the vehicle formation obtained by the on-board sensor, construct a vehicle kinematic model, perform path planning and updating, and obtain the collaborative obstacle avoidance path of the following vehicle; and make a judgment based on the information of the obstacle obtained by the on-board radar. When it is judged that the collaborative obstacle avoidance path can allow the following vehicle to avoid obstacles safely, output the collaborative obstacle avoidance path as a collision-free path and execute step S5; when it is judged that the collaborative obstacle avoidance path cannot allow the following vehicle to avoid obstacles safely, execute step S3.

[0097] In this embodiment, by introducing RRT* and B-spline smooth planning, a smooth collision-free path can be planned based on the starting point and the target point when obstacles are known. However, this planning algorithm cannot be directly applied to the vehicle formation following task at the airport scene, and there is the following problem: it is necessary to consider the situation where the known information of the formation may be insufficient. In order to solve this problem, the embodiment of the present invention adds a prediction step before implementing the RRT* and B-spline smooth planning strategy. The future path of the leading vehicle is predicted by the vehicle kinematic model, and then the predicted planning end point of the following vehicle (the predicted position of the following vehicle) is obtained through path transformation. Therefore, the RRT* and B-spline smooth planning strategy in the first step can be implemented between the current position of the following vehicle and the predicted planning end point to obtain a collision-free path. On this basis, every time the following vehicle travels a certain distance (it is considered that the known information of the formation has been fully updated), a new planning end point is obtained again through prediction, and RRT* and B-spline smooth planning are implemented again. This process is called path end point update, as shown in Figure 2 shown.

[0098] The local path planning adopts the rapidly expanding random tree star algorithm (RRT*). RRT* is a sampling-based planning algorithm, and its core steps include random sampling, selecting neighbor nodes, generating new nodes, collision detection, and reselecting parent nodes and rewiring. The last two steps are additional steps of RRT* compared to the rapidly expanding random tree (RRT), which ensures the asymptotic optimality of the planned path. Given a starting point as the root, RRT* can grow a tree through the above steps and explore the entire sampling space until the newly generated node is close enough to the planning end point. Since RRT* has the rapidity of solving and adaptability to different environments, it is selected as the basic planning algorithm of the method provided by the embodiment of the present invention. In order to reduce the computational burden, in this embodiment, the target point is selected as the sampling point with a probability of p=0.6.

[0099] On this basis, since the planning result of RRT* consists of a series of discrete sampling points, the path it constitutes is not smooth, which brings difficulties to vehicle tracking. Therefore, the B-spline method is used in this implementation to perform smooth optimization of the path, and the B-spline curve is defined as:

[0100]

[0101] Where n is the number of control points, P i represents the i-th control point, B i,p (u) is the p-order basis function, which can be calculated by the Cox-de Boor algorithm:

[0102]

[0103]

[0104] Among them, u represents the curve parameter, u i Represents the i-th node value in the node vector.

[0105] This implementation may use multiple B-spline smoothing paths. For example, a cubic B-spline smoothing path may be used to provide a more optimized and smoother path. In addition, an intermediate point insertion strategy may be used, that is, an additional point is inserted between every two consecutive points in the original sequence, and then the overall B-spline smoothing is performed to make the smoothed path closer to the path planned by RRT*.

[0106] During path planning, each following vehicle in the vehicle formation is judged and planned. In the case of abnormalities on the road ahead and obstacles, the planned paths of the following vehicles can be comprehensively considered and coordinated and adjusted to obtain a collaborative obstacle avoidance path.

[0107] The step S2 specifically includes the following steps.

[0108] Step S2.1, based on the information of the vehicle formation obtained by the on-board sensors, construct the vehicle kinematic model of the leading vehicle:

[0109]

[0110]

[0111]

[0112] Among them, x l (k) and y l (k) is the position coordinate of the leading vehicle at time k, ν l (k) is the linear velocity of the leading vehicle at time k, is the heading angle of the leading vehicle at time k, δ l (k) is the front wheel deflection angle of the leading vehicle at time k, and L is the wheelbase of the vehicle. The wheelbase of the following vehicle and the leading vehicle are considered to be the same. The above information of the leading vehicle comes from the information of the vehicle formation obtained by the on-board sensor.

[0113] Step S2.2, predicting the future predicted position of the leading vehicle based on the vehicle kinematics model of the leading vehicle. Considering the roughly straight-line driving scenario, the leading vehicle has a small turning angle, and it can be approximately considered that δ l (k)≈0. The straight road driving may include slight turning / lane changing operations. The length of the forward path in which the following vehicle can safely avoid obstacles is set as d according to the obstacle information obtained by the vehicle radar. s , the path length of the known path of the leader vehicle as the known information of the formation is dh , then the longitudinal path length d of the predicted path p It is expressed as:

[0114]

[0115] in, is the ceiling function.

[0116] Since the following vehicle and the leading vehicle travel as a platoon, the lengths of their paths are basically the same, so the path length of the above predicted path can be used as the predicted path length of the leading vehicle or the predicted path length of the following vehicle. When the end point of the predicted path is inside the obstacle, a larger d is selected. p The future predicted position of the leading vehicle (i.e., the predicted point of the leading vehicle) can be obtained based on the known information of the formation and the path length of the above predicted path. The forward path length d of the following vehicle that can safely avoid obstacles s It is pre-set based on prior knowledge and can be related to the size and shape of the obstacle. In this step, considering that there may be insufficient known information about the vehicle formation, it is necessary to compensate for this deficiency through prediction. The longitudinal path length d is obtained through prediction. p As the vehicle formation moves forward, the lack of known information in the formation is gradually made up (the formation has more and more known information). h The length is then re-predicted and planned based on the newly acquired information until the lack of known information in the formation is completely made up, and the planned path is the actual path. This continuous prediction process makes the final position of the following vehicle more in line with the needs of smoothly passing the actual road.

[0117] Step S2.3, based on the predicted position of the leading vehicle, the predicted position of the following vehicle (i.e., the predicted point of the following vehicle) is obtained by translation, and then the current position of the following vehicle obtained by the on-board sensor and the predicted position of the following vehicle are used as the starting point and the end point of the prediction planning, respectively, and RRT* and B-spline smoothing planning are performed to obtain the predicted path of the following vehicle as the collaborative obstacle avoidance path. The predicted path can be composed of a series of path points.

[0118] Step S2.4, whenever the following vehicle travels along the predicted path of the following vehicle through a length d h When the path is d, the predicted position of the following vehicle (i.e., the predicted point of the following vehicle) is updated, and the following vehicle is moved along a path of length d. hThe current position after the path and the updated predicted position are used as the starting point and end point of another prediction planning, and RRT* and B-spline smoothing planning are performed to obtain the updated predicted path of the following vehicle as the collaborative obstacle avoidance path. Among them, the length of the predicted path of each updated following vehicle is:

[0119]

[0120] Repeat the prediction update process until the update number When , the prediction operation is stopped. Then, the current position of the leading vehicle is directly translated to obtain the updated future position of the following vehicle. The current position of the following vehicle and the future position of the following vehicle obtained by translation are used as the starting and ending points of the prediction planning. RRT* and B-spline smoothing planning are performed to obtain the predicted path of the following vehicle as the collaborative obstacle avoidance path. When d h ≥ 0.5 days s When , it is considered that the known information of the vehicle formation is sufficient.

[0121] Step S2.5, make a judgment based on the obstacle information obtained by the vehicle-mounted radar. When it is judged that the collaborative obstacle avoidance path can allow the following vehicle to avoid obstacles safely, output the collaborative obstacle avoidance path as a collision-free path and execute step S5; when it is judged that the collaborative obstacle avoidance path cannot allow the following vehicle to avoid obstacles safely, execute step S3.

[0122] It should be understood that this path planning strategy based on the updated predicted position of the following vehicle can periodically adjust the future predicted position of the following vehicle according to the position of the leading vehicle. The smoothness of the updated path can depend on the path of the manned leading vehicle. The smoother the path of the leading vehicle, the smoother the updated path. The appropriate turning and lane changing behavior of the leading vehicle in the vehicle formation can be compensated by adjusting the predicted position of the following vehicle through continuous prediction and updating, thereby adjusting the predicted path of the following vehicle.

[0123] Step S3, when it is determined based on the information obtained by the vehicle-mounted radar that the width of the road ahead of the following vehicle is too narrow to allow the following vehicle in parallel to pass, a way-giving strategy is obtained based on the local following path and the predicted path of the following vehicle; and a judgment is made based on the road ahead information obtained by the vehicle-mounted radar. When it is determined that the way-giving strategy can allow the following vehicle to pass the road ahead safely, the way-giving strategy is output as a collision-free path and step S5 is executed; when it is determined that the way-giving strategy cannot allow the following vehicle to pass the road ahead safely, step S4 is executed.

[0124] Each following vehicle performs collision avoidance planning under the premise of considering the paths of other following vehicles, and obtains the collision avoidance planning path as a yielding strategy, such as Figure 3 shown.

[0125] By introducing the above-mentioned RRT* planning and B-spline smoothing algorithm, a smooth collision-free path is planned according to the starting point and the target point in the case of known obstacles. However, this planning algorithm cannot be directly applied to the vehicle formation following task at the airport scene, and there is still the following problem: for the obstacle avoidance task of the vehicle formation, it is necessary to consider avoiding collisions between the following vehicles. This embodiment of the present invention adopts a vehicle-to-vehicle communication method through a network, so that the side-by-side following vehicles of the vehicle formation (for example, two following vehicles in a triangular formation) can interact with each other about their local following paths or collaborative obstacle avoidance paths for a period of time in the future. When the following vehicle approaches an obstacle, the obstacle avoidance planning algorithm obtains the above-mentioned collaborative obstacle avoidance path through RRT* and B-spline smoothing planning, and when driving in the form of a vehicle formation, the above-mentioned local following path is obtained by path transformation based on the known information of the formation. When a following vehicle in the vehicle formation performs an obstacle avoidance operation, the local following path of another following vehicle will be regarded as an inaccessible area, thus avoiding collisions between following vehicles. At the same time, for a following vehicle in the vehicle formation, if its local following path collides with the cooperative obstacle avoidance path of another following vehicle, the following vehicle is also required to perform the path planning of step S2 to implement the operation of the yield strategy. When the obstacle avoidance paths obtained by the two following vehicles through path planning cannot allow the vehicle formation to safely pass the road ahead, it is necessary to switch to the formation change strategy.

[0126] The step S3 specifically includes the following steps.

[0127] Step S3.1, local following paths and cooperative obstacle avoidance paths between following vehicles arranged side by side in the vehicle formation through vehicle-to-vehicle communication interaction;

[0128] Step S3.2, determining whether a local following path of a following vehicle and a cooperative obstacle avoidance path of another following vehicle collide. If it is determined that no collision occurs, the local following path and the cooperative obstacle avoidance path are used as non-collision paths and step S5 is executed. If it is determined that a collision occurs, the path prediction of step S2 is re-performed on the following vehicle to obtain the cooperative obstacle avoidance path of the following vehicle, wherein the cooperative obstacle avoidance path of the other following vehicle is used as the obstacle information in the prediction update during the obstacle avoidance planning process.

[0129] Step S3.3, obtaining respective cooperative obstacle avoidance paths of the two following vehicles as a way-making strategy;

[0130] Step S3.4, make a judgment based on the road ahead information obtained by the vehicle-mounted radar. When it is judged that the give way strategy can allow the following vehicle to pass the road ahead safely, output the give way strategy as a collision-free path and execute step S5. When it is judged that the give way strategy cannot allow the following vehicle to pass the road ahead safely, execute step S4.

[0131] Step S4, based on the information of the vehicle formation obtained by the on-board sensor, the formation change strategy of the vehicle formation is executed to obtain a collision-free path for the following vehicle, such as Figure 4 The above-mentioned giving way strategy cannot allow the following vehicle to pass safely on a narrow road ahead, so the vehicle formation needs to be deformed into a straight line. The step S4 specifically includes the following steps.

[0132] Step S4.1, transforming the vehicle formation from a non-in-line formation (eg, a triangle) to an in-line formation, thereby including a middle following vehicle and a rear following vehicle.

[0133] Step S4.2, similar to step S2, constructs a vehicle kinematic model and performs predictions to obtain the predicted planning end point in the formation transformation of the following vehicles in the straight line formation, and uses the current position of the following vehicle as the predicted planning starting point. Based on the predicted planning starting point and the predicted planning end point, a predicted path can be obtained by using RRT* and B-spline smoothing prediction, and then the predicted position of the leading vehicle is selected at the entrance of the narrower road ahead on the predicted path, and the predicted position is used as the formation transformation end point of the middle following vehicle in the straight line formation, and the end point of the other last following vehicle in the straight line formation can be directly obtained by translating according to the distance between the formation transformation end point of the middle following vehicle and the following vehicle, such as Figure 4 shown.

[0134] Step S4.3, in this embodiment, the known formation information of the vehicle formation is determined. If the known formation information of the vehicle formation is sufficient, the collision-free path of the following vehicle is obtained by using the B-spline smoothing method with the current position of the following vehicle as the starting point to the formation transformation end point, and step S4.5 is executed. If the known formation information of the vehicle formation is insufficient, the update step of S4.4 is executed.

[0135] Step S4.4, update the formation change termination point of the following vehicle to obtain the collision-free path of the following vehicle. In the case where the formation is changed to a straight formation, only one update operation is required when the leading vehicle passes the predicted position in step S4.2, which can better meet the application requirements. The updated lateral distance is compared with the longitudinal update path length d h Smaller, to avoid excessive adjustment of the vehicle's longitudinal heading angle, making the following vehicle's travel more stable and smooth.

[0136] Assume that the safe lateral distance between the longitudinal center axis of the following vehicle and the edge of the road ahead is d l (Since the current formation is in a straight line, the leading vehicle is also the same). According to the trigger logic of the formation change, the width of the current road can be obtained as s∈[2dl ,4d l By selecting the distance between the front and rear adjacent vehicles in the straight formation, the longitudinal update distance is greater than d h Since the prediction points generated by random sampling and the update points determined by the predicted position of the leading vehicle in this step are located with the same probability in the width (s-2d l ), then the expected value of the lateral update distance is

[0137] E[u a ]=(s-2d l ) / 3.

[0138] For example, in a scenario of an embodiment, assuming that l =1.5m,s=5m,d h =5m. Then the ratio of the longitudinal update distance to the lateral update distance is: It can be seen that the horizontal update distance E[u a Compared with the longitudinal update distance, the algorithm can be implemented in real applications, and the update process is smoother. The longitudinal direction is the forward direction of the vehicle formation, while the lateral movement is caused by the vehicle's obstacle avoidance operation. The lateral update distance is smaller than the longitudinal update distance, which means that the longitudinal heading angle of the vehicle changes less, making the update process easy to implement in a practical sense.

[0139] Step S4.5, after passing through the narrow road, the vehicle formation is inversely transformed, and the straight line formation is converted into a triangle formation.

[0140] A method similar to the above-mentioned formation transformation strategy is used to transform the vehicle formation in a straight line formation back into a non-straight line formation (e.g., a triangle) through an inverse formation transformation, but the obstacle prediction process no longer needs to be considered.

[0141] Step S5, based on the obtained collision-free path of the following vehicle, obtaining a control input of the following vehicle according to the linear model predictive control.

[0142] Step S5.1, based on the obtained collision-free path of the following vehicle and the information of the following vehicle obtained by the on-board sensor, a discrete linear error model of the following vehicle is constructed. The collision-free path of the following vehicle obtained from the previous step is used as the reference trajectory of the following vehicle. The vehicle kinematic model of the following vehicle is linearized near the reference trajectory of the following vehicle to obtain a discrete linear error model:

[0143]

[0144] in, A k , B k, C k It is related to the reference trajectory state quantity and is a time-varying matrix. Indicates the following vehicle error state, represents the following vehicle error input, represents the following vehicle error output, x i (k) represents the position of the following vehicle in the x direction, x f (k) represents the position of the reference point in the x direction, y i (k) represents the position of the following vehicle in the y direction, y f (k) represents the position of the reference point in the y direction, is the reference heading angle, v i (k) is the speed of the following vehicle, v f (k) is the reference speed, δ i (k) is the front wheel deflection angle of the following vehicle, δ f (k) is the reference turning angle. The above information of the following vehicle's position, heading angle, speed, front wheel deflection angle, etc. is obtained through the vehicle-mounted sensor. The above reference point is the coordinate point on the reference path of the following vehicle that is closest to the current position of the following vehicle.

[0145] Step S5.2, construct a linear model predictive controller of the reference trajectory of the following vehicle, and solve the linear model predictive controller to obtain the control input of the following vehicle. The linear model predictive controller is expressed as:

[0146]

[0147] st

[0148]

[0149]

[0150]

[0151]

[0152] Among them, Np represents the prediction time domain, and Is a positive definite matrix, representing the penalty weights of error output and input; and is a vector consisting of the minimum and maximum values ​​of the speed and front wheel deflection angle of the following vehicle, which is used to limit the deviation of the following vehicle's input from the reference trajectory; X i (k) is the state of the following vehicle, X f (k) is the reference state, To predict the output of t steps at time k, is the prediction input for t steps forward at time k, is the prediction error state of t steps predicted at time k; is a symmetric positive definite matrix used to determine the parameters of the obstacle ellipse. Indicates the distance between the following vehicle and the obstacle on the x-axis and y-axis, that is,

[0153]

[0154] Where P o is the position of the obstacle. Ensure that the following vehicle stays away from the vicinity of the obstacle represented by the ellipse. This can be seen as a slight local correction to the expected trajectory to improve the safety of obstacle avoidance. However, the proposed constraint (i.e., the distance between the following vehicle and the obstacle) destroys the convexity of the optimization problem and increases the computational burden, so it is only considered when the following vehicle approaches the obstacle to within a certain distance. The certain distance depends on the size of the obstacle. At time k in each sampling period, the above optimization problem can be solved to obtain the optimal control input sequence Then the control input of the following vehicle is U f (k) represents the reference input, Represents the optimal error input at time k.

[0155] Step S6, transmitting the obtained control input of the following vehicle to the following vehicle, controlling the following vehicle to follow the vehicle, thereby realizing the prediction update planning of the vehicle formation following task.

[0156] In order to verify the practicability and effectiveness of the prediction update planning method for the airport surface vehicle formation following task provided by the implementation mode of the present invention, simulation experiments and actual vehicle experiments were carried out respectively.

[0157] Figure 5 A schematic diagram of the decomposition of a vehicle formation in a V-shaped formation is provided for applying the prediction update planning method for the airport surface vehicle formation following task provided in accordance with an embodiment of the present invention to a vehicle formation. Figure 6 A schematic diagram of an obstacle avoidance route for a vehicle formation in a diamond formation when the prediction update planning method for an airport surface vehicle formation following task provided in accordance with an embodiment of the present invention is applied. Figure 7 A schematic diagram of an obstacle avoidance route for applying the prediction update planning method for an airport surface vehicle formation following task provided in accordance with an embodiment of the present invention to a V-shaped formation.

[0158] In the CARLA simulation environment, more complex formations are considered, namely a diamond formation of four vehicles and a V-formation of five vehicles, to verify the scalability of the proposed algorithm. The expected spacing of a line formation in the formation change scenario is set to 10 meters. For the diamond formation, the longitudinal length of the line formation is 30 meters, instead of 20 meters for the triangle formation. Therefore, the predicted length is extended by 10 meters, which requires the vehicle to perceive obstacles in advance, thus requiring more advanced sensor equipment. For the V-formation, an additional vehicle is introduced, but due to hardware limitations, the predicted length cannot be increased. Therefore, a formation decomposition strategy is adopted to decompose the entire formation into different levels according to the position of the driving direction. For example, the V-formation is decomposed into three levels, such as Figure 5 As shown in the figure. The following vehicles in the first level form a triangle formation with the leading vehicle, and the driving performance is ensured by the triangle formation algorithm proposed by the present invention. The following vehicles in the second level form a triangle formation with the virtual leader in the previous level. It is worth mentioning that for the V-shaped formation using formation decomposition, the order in the formation transformation and inverse transformation follows their hierarchy. The formation effects under the two formations are shown in the figure. Figure 6-7 As shown, the experimental results can show that the prediction update planning algorithm proposed in the present invention has strong scalability.

[0159] Figure 8 A schematic diagram of a cooperative obstacle avoidance driving path for a vehicle formation in a triangular formation by applying the prediction update planning method for an airport surface vehicle formation following task provided in accordance with an embodiment of the present invention. Fig. 9 The diagram is a schematic diagram of applying the predictive update planning method for the airport surface vehicle formation following task provided by an embodiment of the present invention to the updated planning path of a vehicle formation in a triangular formation. Fig.10 A schematic diagram of vehicle speed curves of a vehicle formation in a triangular formation when the prediction update planning method for an airport surface vehicle formation following task provided in accordance with an embodiment of the present invention is applied. Fig.11 A schematic diagram of an obstacle avoidance driving path for applying the prediction update planning method for an airport vehicle formation following task provided in accordance with an embodiment of the present invention to the formation transformation of a triangular formation through a narrow road. Fig.12 The present invention is a schematic diagram of applying the predictive update planning method for the airport surface vehicle formation following task provided by an embodiment of the present invention to the updated planned path of a triangular formation passing through a narrow road.

[0160] In the actual vehicle verification phase, it is important to examine the situation that the known information of the formation is insufficient. The present invention has built a real vehicle hardware platform including three vehicles. The intelligent vehicle obtains real-time speed and positioning based on the inertial navigation system of the global navigation satellite system (GNSS), and shares real-time status and future path information through vehicle-to-vehicle communication. The control input is calculated by the nonlinear optimization solver Ipopt through the formation algorithm of the on-board Intel i7-9700 computer, and is executed online with a period of 0.1 seconds. Considering that the safe obstacle avoidance spacing is 20m, the longitudinal spacing of the vehicle formation (the length of the known information of the formation) is about 7.5m. The results of the formation collaborative obstacle avoidance are as follows. Figure 8-10 As shown in Figure 1, the local planning algorithm is implemented three times, and the planned path is gradually adjusted. The result of the formation change obstacle avoidance is shown in Figure 1. Figure 11-12 As shown in the figure, the target point of each following vehicle is adjusted in the update phase to plan a smooth and collision-free path in the lane change scenario and avoid oscillations from the obstacle avoidance phase to the following driving phase. Specifically, the following vehicle responds to the action of the leading vehicle in advance with the help of the RRT* prediction strategy and adapts to the actual path through the update phase.

[0161] The analysis results show that for the proposed airport field vehicle formation following obstacle avoidance scenario, the prediction update planning method proposed in the embodiment of the present invention can well achieve the obstacle avoidance task under the leadership of manned vehicles, and the updated obstacle avoidance path is relatively smooth and easy to implement in engineering. In addition, through simulation tests and actual vehicle tests, the practicality and scalability of the method provided by the embodiment of the present invention can be verified.

[0162] The above description is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by any technician familiar with the technical field within the technical scope disclosed by the present invention should be covered within the protection scope of the present invention.

[0163] All the above-mentioned optional technical solutions can be combined in any way to form optional embodiments of the present application, and will not be described one by one here. It should be understood that the size of the sequence number of each step in the above-mentioned embodiment does not mean the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiment of the present invention. The above embodiments are only used to illustrate the technical solutions of the present invention, not to limit it; although the present invention is described in detail with reference to the aforementioned embodiments, a person of ordinary skill in the art should understand that it is still possible to modify the technical solutions recorded in the aforementioned embodiments, or to replace some of the technical features therein by equivalents; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present invention, and should be included in the protection scope of the present invention.

Claims

1. A prediction update planning method for the airport surface vehicle formation following task, It is characterized in that The following steps are involved: Step S0: The vehicle formation is a non-in-line formation and includes a leading vehicle and a following vehicle. The information of the leading vehicle and the following vehicle is obtained through the vehicle-mounted sensor to obtain the known path of the leading vehicle, and the information of the road ahead and obstacles is obtained through the vehicle-mounted radar. Step S1, performing path transformation based on the known path of the leading vehicle obtained by the vehicle-mounted sensor, obtaining a local following path of the following vehicle, and making a judgment based on the information obtained by the vehicle-mounted radar. When it is judged that there is no obstacle in the forward direction of the following vehicle, the local following path of the following vehicle is output as a collision-free path and step S5 is executed. When it is judged that there is an obstacle in the forward direction of the following vehicle, step S2 is executed. Step S2, based on the information of the leading vehicle and the following vehicle acquired by the on-board sensor, a vehicle kinematic model is constructed, path planning and updating are performed, a collaborative obstacle avoidance path of the following vehicle is obtained, and a judgment is made based on the information of obstacles acquired by the on-board radar. When it is judged that the collaborative obstacle avoidance path can enable the following vehicle to avoid obstacles safely, the collaborative obstacle avoidance path is output as a collision-free path and step S5 is executed. When it is judged that the collaborative obstacle avoidance path cannot enable the following vehicle to avoid obstacles safely, step S3 is executed. Step S3, when it is determined according to the information obtained by the vehicle-mounted radar that the width of the road ahead of the following vehicle is not wide enough for the following vehicle in parallel to pass, a way-giving strategy is obtained based on the local following path and the predicted path of the following vehicle, and a judgment is made according to the road ahead information obtained by the vehicle-mounted radar. When it is determined that the way-giving strategy can allow the following vehicle to safely pass the road ahead, the way-giving strategy is output as a collision-free path and step S5 is executed. When it is determined that the way-giving strategy cannot allow the following vehicle to safely pass the road ahead, step S4 is executed. Step S4, executing a formation change strategy of the vehicle formation according to the information of the leading vehicle and the following vehicle obtained by the vehicle-mounted sensor, and obtaining a collision-free path for the following vehicle; Step S5, obtaining a control input of the following vehicle according to a linear model predictive control based on the obtained collision-free path of the following vehicle; Step S6, transmitting the obtained control input of the following vehicle to the following vehicle, controlling the following vehicle to follow the vehicle, thereby realizing the prediction update planning of the vehicle formation following task.

2. The prediction update planning method for the airport surface vehicle formation following task according to claim 1, It is characterized in that In step S0, the information of the leading vehicle and the following vehicle acquired by the on-board sensor includes: the known path, current position, speed, heading angle, and front wheel deflection angle of the leading vehicle, and the current position, heading angle, speed, and front wheel deflection angle of the following vehicle; In step S0, the information of the road ahead and obstacles acquired by the vehicle-mounted radar includes: the width of the road ahead, the position of the obstacle, and the shape and size of the obstacle.

3. The prediction update planning method for the airport surface vehicle formation following task according to claim 2, It is characterized in that Step S2 specifically includes the following steps: Step S2.1, constructing a vehicle kinematic model of the leading vehicle based on the information of the vehicle formation obtained by the on-board sensor; Step S2.2, obtaining the predicted position of the leading vehicle based on the vehicle kinematic model of the leading vehicle, and the longitudinal path length of the predicted path is expressed as: Among them, d s It is the length of the path in the forward direction that the following vehicle can safely avoid obstacles, which is set based on the obstacle information obtained by the vehicle-mounted radar. h is the path length of the known path of the leading vehicle, is the ceiling rounding function; Step S2.3, based on the predicted position of the leading vehicle, the predicted position of the following vehicle is obtained by translation, and then the current position of the following vehicle obtained by the on-board sensor and the predicted position of the following vehicle obtained by translation are used as the starting point and the end point of the prediction planning respectively, and RRT* and B-spline smoothing planning are performed to obtain the predicted path of the following vehicle as the collaborative obstacle avoidance path; Step S2.4, whenever the following vehicle travels along the predicted path of the following vehicle through a length d h When the path is d, the predicted position of the following vehicle will be updated, and the following vehicle will travel through a path of length d. h The position after the path and the updated predicted position are used as the starting point and end point of another prediction planning, and RRT* and B-spline smoothing planning are performed to obtain the updated predicted path of the following vehicle as the collaborative obstacle avoidance path. Among them, the predicted path length of each updated following vehicle is: Repeat the prediction update process until the update number When , stop the prediction operation. Next, the current position of the leading vehicle is directly translated to obtain the updated future position of the following vehicle. The current position of the following vehicle and the future position of the following vehicle obtained by translation are used as the starting and ending points of the prediction planning. RRT* and B-spline smoothing planning are performed to obtain the collision avoidance path of the following vehicle as the collaborative obstacle avoidance path. h ≥ 0.5 days s When , it indicates that the formation of the vehicle formation is known to have sufficient information; Step S2.5, make a judgment based on the obstacle information obtained by the vehicle-mounted radar. When it is judged that the collaborative obstacle avoidance path obtained in the above steps can allow the following vehicle to avoid obstacles safely, output the collaborative obstacle avoidance path as a collision-free path and execute step S5. When it is judged that the collaborative obstacle avoidance path cannot allow the following vehicle to avoid obstacles safely, execute step S3.

4. The prediction update planning method for the airport surface vehicle formation following task according to claim 3, It is characterized in that Step S3 specifically includes the following steps: Step S3.1, local following paths or cooperative obstacle avoidance paths between following vehicles arranged side by side in the vehicle formation through vehicle-to-vehicle communication interaction; Step S3.2, determining whether a local following path of a following vehicle and a cooperative obstacle avoidance path of another following vehicle collide. If it is determined that no collision occurs, the local following path and the cooperative obstacle avoidance path are used as non-collision paths and step S5 is executed. If it is determined that a collision occurs, the path obstacle avoidance planning of step S2 is repeated for the following vehicle to obtain the cooperative obstacle avoidance path of the following vehicle, wherein the cooperative obstacle avoidance path of the other following vehicle is used as the obstacle information in the prediction update during the obstacle avoidance planning process. Step S3.3, obtaining respective cooperative obstacle avoidance paths of the two following vehicles as a way-making strategy; Step S3.4, make a judgment based on the road ahead information obtained by the vehicle-mounted radar. When it is judged that the give way strategy can allow the following vehicle to pass the road ahead safely, output the give way strategy as a collision-free path and execute step S5; when it is judged that the give way strategy cannot allow the following vehicle to pass the road ahead safely, execute step S4.

5. The prediction update planning method for the airport surface vehicle formation following task according to claim 4, It is characterized in that Step S4 specifically includes the following steps: Step S4.1, transforming the vehicle formation from a non-in-line formation to an in-line formation, thereby including a middle following vehicle and a rear following vehicle; Step S4.2, constructing a kinematic model of the vehicle and performing prediction, obtaining a predicted planning end point in the formation transformation of the following vehicles in the straight line formation, and taking the current position of the following vehicle as the prediction planning start point, based on the prediction planning start point and the prediction planning end point, using RRT* and B-spline smoothing prediction to estimate the predicted position of the leading vehicle at the entrance of the road ahead, and using the predicted position as the formation transformation end point of the middle following vehicle in the straight line formation, and the formation transformation end point of the last following vehicle in the straight line formation is obtained by translation according to the distance between the formation transformation end point of the middle following vehicle and the following vehicle; Step S4.3, judging the known formation information of the vehicle formation, if the known formation information of the vehicle formation is sufficient, obtaining the collision-free path of the following vehicle by using the B-spline smoothing method from the current position of the following vehicle as the starting point to the formation transformation end point and executing step S4.5, if the known formation information of the vehicle formation is insufficient, executing the updating step of S4.4; Step S4.4, the predicted planned end point of the following vehicle is updated to obtain the collision-free path of the following vehicle. When the leading vehicle passes the predicted position in S4.2, the update operation is performed, wherein the width of the front road as a narrow road is s∈[2d l ,4d l ], by selecting the distance between the front and rear adjacent vehicles in the straight formation, so that the longitudinal update distance is greater than d h , the expected value of the horizontal update distance is E[u a ]=(s-2d l ) / 3 Among them, d l To maintain a safe lateral distance between the longitudinal centre axis of the following vehicle and the edge of the road ahead; Step S4.5, after passing through the narrow road, the vehicle formation is inversely transformed, and the straight-line formation is converted back to a non-straight-line formation.

6. The prediction update planning method for the airport surface vehicle formation following task according to claim 5, It is characterized in that Step S5 specifically includes the following steps: Step S5.1, based on the obtained collision-free path of the following vehicle and the information of the following vehicle obtained by the on-board sensor, a discrete linear error model of the following vehicle is constructed, the obtained collision-free path of the following vehicle is used as a reference trajectory of the following vehicle, and the vehicle kinematic model of the following vehicle is linearized near the reference trajectory of the following vehicle to obtain a discrete linear error model: in, A k , B k , C k Related to the reference trajectory state quantity, it is a time-varying matrix, Indicates the following vehicle error state, represents the following vehicle error input, represents the following vehicle error output, x i (k) represents the position of the following vehicle in the x direction, x f (k) represents the position of the reference point in the x direction, y i (k) represents the position of the following vehicle in the y direction, y f (k) represents the position of the reference point in the y direction, is the reference heading angle, v i (k) is the speed of the following vehicle, v f (k) is the reference speed, δ i (k) is the front wheel deflection angle of the following vehicle, δ f (k) is the reference turning angle. The state information of the following vehicle is obtained through the vehicle-mounted sensor. The reference point is the coordinate point on the reference path of the following vehicle that is closest to the current position of the following vehicle. Step S5.2, construct a linear model predictive controller of the reference trajectory of the following vehicle, and solve the linear model predictive controller to obtain the control input of the following vehicle. The linear model predictive controller is expressed as: Among them, Np represents the prediction time domain, and Is a positive definite matrix, representing the penalty weights of error output and input; and is a vector consisting of the minimum and maximum values ​​of the speed and front wheel deflection angle of the following vehicle; X i (k) is the state of the following vehicle, X f (k) is the reference state, To predict the output of t steps at time k, is the prediction input for t steps forward at time k, is the prediction error state of t steps predicted at time k; is a symmetric positive definite matrix used to determine the parameters of the obstacle ellipse. represents the distance between the following vehicle and the obstacle on the x-axis and y-axis, and Among them, P o is the position of the obstacle, the constraint Ensure that the following vehicle stays away from the vicinity of the obstacle represented by the ellipse, At time k in each sampling cycle, solve the above optimization problem and obtain the optimal control input sequence Then the control input of the following vehicle is Among them, U f (k) represents the reference input, Represents the optimal error input at time k.

Citation Information

Patent Citations

  • Unmanned vehicle control method and related device

    CN111833628A

  • Multi-unmanned aerial vehicle sliding mode formation control method based on data driving

    CN112925347A

  • Vehicle formation obstacle avoidance method based on graph theory and improved first-order algorithm

    CN115774449A

  • Vehicle queue collaborative prediction control method in curve scene

    CN117523813A

  • Intelligent vehicle formation driving path planning method based on pilot followers

    CN118034321A

Cited By

  • Unmanned vehicle formation trajectory planning control system and method

    CN119247965A

  • Unmanned vehicle formation trajectory planning control system and method

    CN119247965B