Parking path planning method and device of semitrailer and terminal equipment

Abstract

The invention provides a parking path planning method and device of a semitrailer and terminal equipment. The method comprises the steps that the initial vehicle state, the target parking position and the parking area of the semitrailer are obtained; converting the semitrailer system model corresponding to the semitrailer into an equivalent single vehicle model; wherein the trailer rear axle center of the semitrailer is determined as the rear axle center of the single-vehicle model, the center distance between the hinged point of the tractor of the semitrailer and the trailer and the rear axle center of the trailer is determined as the single-vehicle axle distance in the single-vehicle model, and the tractor is determined as the single-vehicle wheel of the single-vehicle model; determining a model node path of the single vehicle model in the parking area based on a mixed A * algorithm by using the initial vehicle state and the target parking position; and determining the parking path of the tractor according to the model node path, the corresponding relationship between the semitrailer system model and the single vehicle model and the kinematics relationship between the tractor and the trailer. According to the method, the parking safety and efficiency of the semitrailer can be improved.

Description

Technical Field

[0001] The present application relates to the field of intelligent driving technology, and in particular to a parking path planning method, apparatus, and terminal device for a semi-trailer. Background Art

[0002] With the booming development of the transportation industry and the continuous improvement of road infrastructure, the market demand for semi-trailers is also growing. In a semi-trailer, the driving front part is the tractor, and the non-driving part used for carrying cargo is the trailer; the tractor and trailer are connected in an articulated manner.

[0003] Current technical solutions use a hybrid A* algorithm to perform position search based on a semi-trailer system model to determine the parking path for the tractor. However, this approach cannot guarantee that the trailer's posture will meet the required posture when the tractor parks according to the parking path.

[0004] Therefore, how to plan the parking path of a semi-trailer so that the semi-trailer can take into account the vehicle positions of the tractor and trailer when parking according to the parking path and ensure parking safety is a technical problem that technicians in this field currently need to solve. Summary of the Invention

[0005] The purpose of this application is to provide a parking path planning method, device, terminal device and computer-readable storage medium for a semi-trailer, so as to enable the semi-trailer to take into account the vehicle positions of the tractor and trailer when parking according to the parking path, thereby ensuring parking safety.

[0006] In a first aspect, the present application provides a method for planning a parking path for a semi-trailer. The method comprises:

[0007] Obtaining an initial vehicle state, a target parking position, and a parking area of ​​the semi-trailer; the initial vehicle state includes an initial parking position and an initial heading angle;

[0008] Converting the semitrailer system model corresponding to the semitrailer into an equivalent single-vehicle model; wherein the center of the rear axle of the semitrailer is determined as the rear axle center of the single-vehicle model, the center distance from the hinge point of the tractor of the semitrailer and the trailer to the center of the rear axle of the trailer is determined as the single-vehicle wheelbase in the single-vehicle model, and the tractor is determined as the single wheel of the single-vehicle model;

[0009] Determining a model node path of the bicycle model within the parking area based on a hybrid A* algorithm using the initial vehicle state and the target parking position;

[0010] The parking path of the tractor is determined according to the model node path, the corresponding relationship between the semi-trailer system model and the single vehicle model, and the kinematic relationship between the tractor and the trailer.

[0011] In a specific embodiment, the determining of the model node path of the bicycle model within the parking area based on a hybrid A* algorithm using the initial vehicle state and the target parking space position includes:

[0012] Determining a current node state of the single vehicle model according to the initial vehicle state of the semi-trailer system model; the current node state includes a current node position and a current node heading angle;

[0013] Determine a plurality of node wheel angles corresponding to the current node position according to a preset sampling interval;

[0014] For each of the node wheel angles corresponding to the current node position, determining a predicted node state of a next node position corresponding to the current node position according to the current node state of the current node position;

[0015] Determining whether to determine the next node position as a candidate node position according to the predicted node state, and adding the candidate node position to a set of candidate node positions;

[0016] After traversing the wheel angles of each node at the current node position, an updated set of candidate node positions is obtained;

[0017] Determine a target alternative node position from the set of alternative node positions, determine the target alternative node position as the current node position, and return to the step of determining multiple node wheel angles corresponding to the current node position according to a preset sampling interval; until the model node path of the bicycle model in the parking area is determined according to the current node position and the target parking node position; the target parking node position is a node position corresponding to the target parking position determined based on the correspondence between the semi-trailer system model and the bicycle model.

[0018] In a specific embodiment, determining whether to determine the next node position as a candidate node position according to the predicted node state, and adding the candidate node position to a set of candidate node positions includes:

[0019] determining whether the next node position is a traversed node position, and determining whether the next node position collides with an obstacle in the parking area based on the predicted node state and obstacle information;

[0020] If the next node position is not the traversed node position and the next node position does not collide with the obstacle, the next node position is determined as the candidate node position.

[0021] In a specific embodiment, determining a target candidate node position from the candidate node position set and determining the target candidate node position as the current node position includes:

[0022] Obtaining the heuristic function values ​​corresponding to the positions of the candidate nodes;

[0023] A target candidate node position is determined from the candidate node position set according to each of the heuristic function values, the target candidate node position is determined as the current node position, and the candidate node position is added to the candidate node position set.

[0024] In a specific embodiment, the step of determining a target candidate node position from the set of candidate node positions, determining the target candidate node position as the current node position, and returning to the step of determining a plurality of node wheel angles corresponding to the current node position according to a preset sampling interval; until a model node path of the bicycle model within the parking area is determined according to the current node position and the target parking node position, includes:

[0025] Determine a target candidate node position from the candidate node position set, and determine the target candidate node position as the current node position;

[0026] If the current node position is the target parking node position, determining a model node path of the bicycle model in the parking area according to the traversed current node positions and the target parking node position;

[0027] If the current node position is not the target parking node position, determining whether a model node path can be determined within the parking area based on the current node position and the target parking node position;

[0028] If the model node path is not determined, the process returns to the step of determining a plurality of node wheel angles corresponding to the current node position according to a preset sampling interval.

[0029] In a specific embodiment, determining a model node path within the parking area according to the current node position and the target parking node position includes:

[0030] Determining a first path curve according to the traversed current node positions;

[0031] Determine the polynomial expression of the helix;

[0032] Determining a state vector corresponding to the spiral line according to a node state change of the bicycle model;

[0033] determining a parametric equation of the spiral according to the current node position, the target parking node position, and the state vector, and determining a second path curve within the parking area according to the parametric equation;

[0034] Determining a model node path within the parking area based on the first path curve and the second path curve. In a specific embodiment, determining a model node path within the parking area based on the current node position and the target parking node position includes:

[0035] Determine the expressions of the first helix and the second helix;

[0036] Determining the state vectors of the first helix and the second helix according to the node state change of the bicycle model;

[0037] determining a first parametric equation of the first spiral according to the current node position, the initial node position, and the state vector, and determining a first path curve within the parking area according to the first parametric equation;

[0038] determining a second parametric equation of the second spiral according to the current node position, the target parking node position, and the state vector, and determining a second path curve within the parking area according to the second parametric equation;

[0039] A model node path within the parking area is determined according to the first path curve and the second path curve.

[0040] In a specific embodiment, determining the parking path of the tractor according to the model node path, the correspondence between the semi-trailer system model and the single vehicle model, and the kinematic relationship between the tractor and the trailer includes:

[0041] Determining the angle between the tractor and the trailer and the wheel angle of the tractor corresponding to each path point on the model node path according to the correspondence between the model node path and the semi-trailer system model and the single vehicle model;

[0042] If neither the included angle nor the wheel turning angle of the tractor exceeds a corresponding maximum angle threshold, determining a candidate parking path for the tractor based on the model node path, and determining whether the candidate parking path for the tractor collides with an obstacle;

[0043] If the candidate parking path of the tractor does not collide with the obstacle, determining the candidate parking path as the parking path of the tractor;

[0044] If the included angle or the wheel angle of the tractor exceeds the maximum angle threshold, and / or the candidate parking path of the tractor collides with the obstacle, the process returns to the step of determining the wheel angles of multiple nodes corresponding to the current node position according to the preset sampling interval.

[0045] In a second aspect, the present application also provides a parking path planning device for a semi-trailer. The device comprises:

[0046] An acquisition module is used to acquire the initial vehicle state, target parking position and parking area of ​​the semi-trailer; the initial vehicle state includes the initial parking position and the initial heading angle;

[0047] an equivalent module, configured to convert the semitrailer system model corresponding to the semitrailer into an equivalent single-vehicle model; wherein the center of the rear axle of the semitrailer is determined as the rear axle center of the single-vehicle model, the center distance from the hinge point of the tractor of the semitrailer and the trailer to the center of the rear axle of the trailer is determined as the single-vehicle wheelbase in the single-vehicle model, and the tractor is determined as the single wheel of the single-vehicle model;

[0048] a position search module, configured to determine a model node path of the bicycle model within the parking area based on a hybrid A* algorithm using the initial vehicle state and the target parking position;

[0049] A path determination module is used to determine the parking path of the tractor according to the model node path, the corresponding relationship between the semi-trailer system model and the single vehicle model, and the kinematic relationship between the tractor and the trailer.

[0050] In a third aspect, the present application further provides a terminal device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of the above method when executing the computer program.

[0051] In a fourth aspect, the present application further provides a computer-readable storage medium storing a computer program, which implements the steps of the above method when executed by a processor.

[0052] The present application provides a parking path planning method for a semitrailer, which converts the semitrailer system model corresponding to the semitrailer into an equivalent single-vehicle model, treats the semitrailer's trailer as the vehicle body in the single-vehicle model, and treats the semitrailer's tractor as the wheel in the single-vehicle model; then, using the initial vehicle state and the target parking position, a model node path of the single-vehicle model within the parking area is determined based on a hybrid A* algorithm; that is, when determining the parking path of the tractor, the method first determines the model node path based on the single-vehicle model, and then determines the parking path of the tractor based on the model node path, the correspondence between the semitrailer system model and the single-vehicle model, and the kinematic relationship between the tractor and the trailer; therefore, when the tractor parks according to the parking path, it can also take into account the vehicle posture of the trailer, ensuring parking safety. Furthermore, converting the semitrailer system model corresponding to the semitrailer into an equivalent single-vehicle model can reduce the vehicle state dimension when performing position search using the hybrid A* algorithm, thereby improving the efficiency of parking path planning. In summary, this method can improve the parking safety and efficiency of semi-trailers.

[0053] It can be understood that the parking path planning device, terminal device and computer-readable storage medium for a semi-trailer provided in the embodiments of the present application have the same beneficial effects as the above-mentioned parking path planning method for a semi-trailer, and will not be repeated here. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] In order to more clearly illustrate the specific implementation methods of the present application or the technical solutions in the prior art, the following is a brief introduction to the drawings required for use in the specific implementation methods or the description of the prior art. Obviously, the drawings described below are some implementation methods of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without any creative work.

[0055] Figure 1 A flowchart of a method for planning a parking path for a semi-trailer provided in an embodiment of the present application;

[0056] Figure 2 A schematic diagram of a semi-trailer system model corresponding to a semi-trailer provided in an embodiment of the present application;

[0057] Figure 3 A schematic diagram of a single vehicle model equivalent to a semi-trailer system model provided in an embodiment of the present application;

[0058] Figure 4 A schematic diagram of a parking path planning method for a semi-trailer provided in an embodiment of the present application;

[0059] Figure 5A flowchart of another method for planning a parking path for a semi-trailer provided in an embodiment of the present application, comprising the steps of: utilizing an initial vehicle state and a target parking space position to determine a model node path of a single vehicle model within a parking area based on a hybrid A* algorithm;

[0060] Figure 6 A flowchart of another method for planning a parking path for a semi-trailer provided in an embodiment of the present application, wherein the step of determining whether to determine the next node position as an alternative node position according to the predicted node state;

[0061] Figure 7 FIG2 is a schematic structural diagram of a parking path planning device for a semi-trailer provided in an embodiment of the present application;

[0062] Figure 8 A schematic diagram of the structure of a terminal device provided in an embodiment of the present application. DETAILED DESCRIPTION

[0063] In the following description, specific details such as specific system structures and techniques are provided for purposes of illustration rather than limitation to facilitate a thorough understanding of the embodiments of the present application. However, it will be apparent to those skilled in the art that the present application may be implemented in other embodiments without these specific details. In other cases, detailed descriptions of well-known systems, devices, circuits, and methods are omitted to avoid obscuring the description of the present application with unnecessary detail.

[0064] It should be understood that when used in the present specification and the appended claims, the term "comprising" indicates the presence of described features, integers, steps, operations, elements and / or components, but does not preclude the presence or addition of one or more other features, integers, steps, operations, elements, components and / or collections thereof.

[0065] It will also be understood that the term "and / or" used in this specification and the appended claims refers to and includes any and all possible combinations of one or more of the associated listed items.

[0066] As used in this specification and the appended claims, the term "if" can be interpreted as "when" or "upon" or "in response to determining" or "in response to detecting," depending on the context. Similarly, the phrase "if it is determined" or "if [described condition or event] is detected" can be interpreted as meaning "upon determination" or "in response to determining" or "upon detection of [described condition or event]" or "in response to detecting [described condition or event]," depending on the context.

[0067] In addition, in the description of the present application specification and the appended claims, the terms "first", "second", "third", etc. are only used to distinguish the descriptions and cannot be understood as indicating or implying relative importance.

[0068] References to "one embodiment" or "some embodiments" in this specification mean that one or more embodiments of the present application include a particular feature, structure, or characteristic described in conjunction with that embodiment. Thus, phrases such as "in one embodiment," "in some embodiments," "in other embodiments," and "in other embodiments" appearing in various places in this specification do not necessarily refer to the same embodiment, but rather mean "one or more but not all embodiments," unless otherwise specifically emphasized. The terms "including," "comprising," "having," and variations thereof all mean "including but not limited to," unless otherwise specifically emphasized. "Multiple" means "two or more."

[0069] An embodiment of the present application provides a method for planning a parking path for a semi-trailer, which can be executed by a processor of a terminal device when running a corresponding computer program.

[0070] Figure 1 This is a flowchart of a method for planning a parking path for a semi-trailer provided in an embodiment of the present application. For ease of illustration, only the portion relevant to this embodiment is shown. The method provided in this embodiment includes the following steps:

[0071] S100: Acquire the initial vehicle state, target parking position, and parking area of ​​the semi-trailer; the initial vehicle state includes the initial parking position and the initial heading angle.

[0072] The initial parking position refers to the starting point of the planned parking path; the target parking position refers to the end point of the planned parking path; and the parking area refers to the limited path search range. It should be understood that the planned parking path is a path that allows the semi-trailer to smoothly move from the initial parking position to the target parking position without exceeding the parking area.

[0073] Specifically, the initial parking position of the semi-trailer can be obtained using the semi-trailer's positioning device, and the target parking position and parking area can be obtained using the vehicle's map module. In actual application, a pre-set parking area can be obtained, or the corresponding parking area can be determined during parking path planning.

[0074] S200: Convert the semi-trailer system model corresponding to the semi-trailer into an equivalent single-vehicle model; wherein, the center of the rear axle of the trailer of the semi-trailer is determined as the rear axle center of the single-vehicle model, the center distance from the hinge point of the tractor and trailer of the semi-trailer to the center of the rear axle of the trailer is determined as the single-vehicle wheelbase in the single-vehicle model, and the tractor is determined as the single-vehicle wheel of the single-vehicle model.

[0075] like Figure 2 A schematic diagram of a semi-trailer system model corresponding to a semi-trailer provided in an embodiment of the present application; Figure 3 A schematic diagram of a single vehicle model equivalent to a semi-trailer system model provided in an embodiment of the present application; Figure 4 A schematic diagram of a parking path planning method for a semi-trailer provided in an embodiment of the present application.

[0076] Specifically, the entire parking area is divided into four dimensions according to a certain grid accuracy to obtain a four-dimensional grid space; each grid represents a certain area according to the division accuracy. One of the small areas.

[0077] For the semi-trailer system model, the four-dimensional state Represents the vehicle state corresponding to the semi-trailer system model; where (x, y) is the position coordinate of the center of the rear axle of the tractor in the semi-trailer, that is, the parking position of the semi-trailer. represents the heading angle of the tractor, Indicates the heading angle of the trailer. Figure 2 In (x t ,y t ) is the position coordinate of the center of the rear axle of the trailer; L1 is the wheelbase of the trailer, that is, the distance from the hinge point of the tractor and trailer to the center of the rear axle of the trailer (x t ,y t ) center distance; L2 is the wheelbase of the tractor; H is the center distance from the hinge point of the tractor and trailer to the center of the tractor's rear axle (x, y), which is generally positive behind the center of the tractor's rear axle and negative in front; θ is the wheel angle of the tractor.

[0078] Combine Figures 2 to 4 As shown in the figure, when converting the semi-trailer system model corresponding to the semi-trailer into an equivalent single vehicle model, the center of the rear axle of the semi-trailer (x t ,y t ) is determined as the rear axle center of the single vehicle model, and the hinge point of the tractor and trailer of the semi-trailer is connected to the rear axle center of the trailer (x t ,y t) is determined as the wheelbase of a single vehicle in the single vehicle model, and the tractor is determined as the wheel of a single vehicle in the single vehicle model; that is, the trailer of the semi-trailer is regarded as the body of the single vehicle model, and the tractor of the semi-trailer is regarded as the wheel of the single vehicle model. Determine the node wheel angle of the single vehicle model; the vehicle state corresponding to the four-dimensional space of the semi-trailer system Convert to the vehicle state corresponding to the three-dimensional space That is, the node state corresponding to the bicycle model.

[0079] S300: Using the initial vehicle state and the target parking position, determine the model node path of the single vehicle model in the parking area based on the hybrid A* algorithm.

[0080] Among them, the hybrid A* algorithm is a kinematics-based search method, which is suitable for robots and cars with motion constraints; the model node path is the parking path corresponding to the bicycle model in the parking area.

[0081] Specifically, based on the correspondence between the semi-trailer system model and the single-vehicle model, the initial node state of the single-vehicle model corresponding to the initial vehicle state of the semi-trailer is determined; the initial node state includes the initial node position and the initial node heading angle; based on the correspondence between the semi-trailer system model and the single-vehicle model, the target parking node position corresponding to the target parking position is determined; then the initial node position is used as the starting position of the parking path of the single-vehicle model, that is, the initial node position is determined as the current node position, the initial node heading angle is determined as the current node heading angle, a position search is performed in the parking area based on the current node position, and the current node position of the single-vehicle model is iteratively updated; the model node path corresponding to the single-vehicle model in the parking area is determined according to the initial node position, the current node positions determined during the position search process, and the target parking node position.

[0082] S400: Determine a parking path for the tractor according to the model node path, the correspondence between the semi-trailer system model and the single vehicle model, and the kinematic relationship between the tractor and the trailer.

[0083] Specifically, after determining the model node path corresponding to the single-vehicle model, the trailer path corresponding to the rear axle center of the trailer in the semi-trailer system model is determined based on the correspondence between the semi-trailer system model and the single-vehicle model; then, the path point of the tractor is determined based on the kinematic relationship between the tractor and the trailer to obtain the parking path of the tractor.

[0084] More specifically, combined Figure 2 and Figure 3 Based on the geometric relationship shown, the path points corresponding to the parking path of the tractor are determined as follows:

[0085]

[0086] Among them, (x, y) is the path point corresponding to the parking path of the tractor, which corresponds to the parking path of the tractor, that is, the position coordinates of the center of the rear axle of the tractor in the semi-trailer; (x t ,y t ) is the position coordinate of the center of the rear axle of the trailer, which corresponds to the trailer path of the trailer, that is, the path point corresponding to the model node path of the single vehicle model; represents the heading angle of the tractor, represents the heading angle of the trailer; L1 is the wheelbase of the trailer; H is the center distance (x, y) from the hinge point of the tractor and trailer to the center of the tractor's rear axle.

[0087] In actual applications, after determining the parking path of the tractor, the driving control information such as the tractor's speed and wheel angle is determined based on the correspondence between the semi-trailer system model and the single vehicle model, as well as the kinematic relationship between the tractor and trailer in the semi-trailer system model. The vehicle controller is then used to control the semi-trailer according to the driving control information, so that the trailer drives to the target parking position according to the parking path, completing automatic parking.

[0088] The embodiment of the present application provides a parking path planning method for a semi-trailer, which converts the semi-trailer system model corresponding to the semi-trailer into an equivalent single-vehicle model, regards the semi-trailer's trailer as the body in the single-vehicle model, and regards the semi-trailer's tractor as the wheel in the single-vehicle model; then, using the initial vehicle state and the target parking position, the model node path of the single-vehicle model in the parking area is determined based on the hybrid A* algorithm; that is, when determining the parking path of the tractor, the method first determines the model node path based on the single-vehicle model, and then determines the parking path of the tractor based on the model node path, the correspondence between the semi-trailer system model and the single-vehicle model, and the kinematic relationship between the tractor and the trailer; therefore, when the tractor parks according to the parking path, it can also take into account the vehicle posture of the trailer to ensure parking safety. In addition, converting the semi-trailer system model corresponding to the semi-trailer into an equivalent single-vehicle model can reduce the vehicle state dimension when using the hybrid A* algorithm for position search, and improve the efficiency of parking path planning. In summary, the present method can improve the parking safety and efficiency of semi-trailers.

[0089] like Figure 5 This is a flowchart of another method for planning a parking path for a semi-trailer provided in an embodiment of the present application, wherein the steps include: using the initial vehicle state and the target parking space position, and determining the model node path of a single vehicle model within the parking area based on a hybrid A* algorithm. Based on the above embodiment, this embodiment further illustrates and optimizes the technical solution. Specifically, in this embodiment, Figure 5As shown in the figure, the model node path of the single-vehicle model in the parking area is determined based on the hybrid A* algorithm using the initial vehicle state and the target parking space position, including:

[0090] Determine the current node state of the single vehicle model according to the initial vehicle state of the semi-trailer system model; the current node state includes the current node position and the current node heading angle;

[0091] Determine the wheel angles of multiple nodes corresponding to the current node position according to a preset sampling interval;

[0092] For each node wheel angle corresponding to the current node position, determine the predicted node state of the next node position corresponding to the current node position according to the current node state of the current node position;

[0093] Determine whether to determine the next node position as a candidate node position according to the predicted node state, and add the candidate node position to the candidate node position set;

[0094] After traversing the wheel angles of each node at the current node position, an updated set of candidate node positions is obtained;

[0095] Determine the target alternative node position from the set of alternative node positions, determine the target alternative node position as the current node position, and return to the step of determining multiple node wheel angles corresponding to the current node position according to the preset sampling interval; until the model node path of the bicycle model in the parking area is determined according to the current node position and the target parking node position; the target parking node position is the node position corresponding to the target parking position determined based on the correspondence between the semi-trailer system model and the bicycle model.

[0096] Specifically, the initial node state of the single-vehicle model is determined based on the correspondence between the semi-trailer system model and the single-vehicle model; the initial node state includes the initial node position and the initial node heading angle. The initial node position is the starting position for path planning based on the single-vehicle model, and the initial node heading angle is the heading angle of the trailer.

[0097] After determining the initial node position of the bicycle model, the initial node position is determined as the current node position, and the initial node heading angle is determined as the current node heading angle; then, based on the current node position, the maximum wheel turning angle constraint (-θ max ,θ max ), sampling is performed under the maximum wheel angle constraint according to the preset sampling interval to obtain the corresponding multiple node wheel angles

[0098] Moreover, based on the corresponding relationship between the semi-trailer system model and the single vehicle model, the preset search step length Δs corresponding to the semi-trailer system model is converted into the search step length Δs corresponding to the single vehicle model. t ;The angle between the tractor and the trailer Determine the node wheel angle of the bicycle model. Based on the bicycle model, for each node wheel angle of the current node position, determine the predicted node state of the next node position corresponding to the current node position according to the current node state of the current node position. Specifically, based on the current node state of the current node position, with a pre-set search step size Δs t The state change of the bicycle model corresponding to the next node position is calculated according to the following formula, and then the predicted node state is determined based on the current node state and the state change.

[0099]

[0100] in, Indicates the state change of the bicycle model at the next node position and the current node position; three-dimensional vector Indicates the current node state of the bicycle model at the current node position; more specifically, (x t ,y t ) represents the current node position of the bicycle model, that is, the path point corresponding to the model node path of the bicycle model (the position coordinates of the center of the trailer's rear axle); It represents the initial node heading angle of the bicycle model at the current node position (the heading angle of the trailer); L1 is the wheelbase of the bicycle model.

[0101] The candidate node position set includes candidate node positions determined at each current node position; a candidate node position is a position that can be used as the current node position in the next position search. In this embodiment, a pre-set judgment rule is obtained, and a determination is made based on the judgment rule as to whether the predicted node state of the next node position meets the requirements for determining the next node position as a candidate node position. If so, the next node position is determined as a candidate node position and is added to the candidate node position set; otherwise, the next node position is determined as a non-traversable node position.

[0102] After traversing the wheel angles of each node corresponding to the current node position is completed, an updated set of alternative node positions is obtained; then the target alternative node position is determined from the set of alternative node positions, and the target alternative node position is determined as the current node position, and it is determined whether the model node path of the bicycle model in the parking area can be determined in the parking area based on the current node position; if the model node path of the bicycle model in the parking area is determined, the model node path is output; otherwise, the step of determining the multiple node wheel angles corresponding to the current position according to the preset sampling interval is returned, that is, the position search is continued based on the updated current node position until the model node path is output.

[0103] In this embodiment, in the process of determining the model node path of the bicycle model in the parking area based on the hybrid A* algorithm, the search direction is determined according to the node wheel angle of the bicycle model based on the kinematic relationship, and for each node wheel angle corresponding to the current node position, the predicted node state of the next node position corresponding to the current node position is determined according to the current node state of the current node position, and whether to determine the next node position as an alternative node position is determined according to the predicted node state, so that the position search can be performed iteratively based on the alternative node position subsequently. The model node path of the bicycle model can be determined efficiently and conveniently, thereby improving the efficiency of parking path planning.

[0104] Based on the above embodiment, this embodiment further illustrates and optimizes the technical solution. Specifically, in this embodiment, determining whether to determine the next node position as a candidate node position based on the predicted node state and adding the candidate node position to the candidate node position set includes:

[0105] determining whether the next node position is a traversed node position, and determining whether the next node position collides with an obstacle in the parking area based on the predicted node state and obstacle information;

[0106] If the next node position is not a traversed node position and the next node position does not collide with an obstacle, the next node position is determined as a candidate node position and the candidate node position is added to the candidate node position set.

[0107] The traversed node position is the node position that has been searched as the current node position; the next node position is compared with the pre-stored traversed node positions to determine whether the next node position is a traversed node position.

[0108] The obstacle information refers to the position information corresponding to the obstacle in the parking area; the next node position is compared with the obstacle information to determine whether the next node position collides with the obstacle in the parking area.

[0109] Specifically, if the next node position is not a traversed node position and the next node position does not collide with an obstacle, the next node position is determined as an alternative node position; otherwise, that is, the next node position is a traversed node position, or the next node position collides with an obstacle, the next node position is ignored.

[0110] In a specific embodiment, three node sets (Open_list set, Close_list set and Open_pq set) are pre-initialized to store node position information during the position search process; wherein, the Open_list set is used to store all candidate node positions determined during the position search process, that is, the Open_list set includes traversed node positions and untraversed candidate node positions; the Close_list set is used to store traversed node positions, that is, the node positions that have been used as the current node positions for position search; the Open_pq set is the candidate node position set, which is used to store the candidate node positions that have not been traversed in the Open_list set, that is, the corresponding set after the Close_list set is deleted compared to the Open_list set, and the candidate node positions in the Open_pq set are sorted according to certain rules.

[0111] Figure 6 A flowchart of another method for planning a parking path for a semi-trailer provided in an embodiment of the present application, wherein the steps of determining whether to determine the next node position as an alternative node position according to the predicted node state. Figure 6 As shown, when the predicted node state corresponding to the next node position is determined, whether the next node position is a node position in the Open_list set is judged according to the predicted node state; if the next node position is a node position in the Open_list set, it means that the next node position has been stored in the Open_list set as an alternative node position in the previous traversal process, so the next node position is ignored; if the next node position is not a node position in the Open_list set, whether the next node position collides with an obstacle in the parking area is determined according to the predicted node state of the next node position; if the next node position collides with an obstacle in the parking area, the next node position is ignored; if the next node position does not collide with an obstacle in the parking area, the next node position is determined as an alternative node position, and the alternative node position is stored in the Open_list set and the Open_pq set (alternative node position set).

[0112] After traversing the wheel angles of each node at the current node position, the updated Open_list set and Open_pq set (alternative node position set) are obtained, and the current node position is stored in the Close_list set; the target alternative node position is determined from the Open_pq set (alternative node position set), and the target alternative node position is determined as the current node position; the target alternative node position (updated current node position) is deleted from the Open_pq set (alternative node position set).

[0113] This embodiment determines whether to determine the next node position as an alternative node position based on whether the next node position is a traversed node position and whether the next node position collides with an obstacle in the parking area, thereby ensuring the availability of each alternative node position, thereby improving the efficiency of determining the parking path of the tractor in the semi-trailer and improving the efficiency of parking path planning.

[0114] Based on the above embodiment, this embodiment further illustrates and optimizes the technical solution. Specifically, in this embodiment, determining a target candidate node position from a set of candidate node positions and determining the target candidate node position as the current node position includes:

[0115] Get the heuristic function value corresponding to each candidate node position;

[0116] The target candidate node position is determined from the candidate node position set according to each heuristic function value, and the target candidate node position is determined as the current node position.

[0117] Specifically, after the next node position is determined as the candidate node position, the heuristic function value corresponding to the candidate node position is calculated based on the heuristic function f, and the candidate node position and the corresponding heuristic function value are stored in the Open_list set and the Open_pq set.

[0118] The heuristic function f includes a cost term g and a heuristic term h. The cost term g is typically set based on actual needs, such as adding costs for large turns and excessive steering rates to prevent the semi-trailer from engaging in these behaviors. The heuristic term h is typically the distance cost from the semi-trailer's current parking space to the target parking space, ensuring that the semi-trailer continuously approaches the target parking space. Specifically, the distance cost can be calculated using distance calculation methods such as Euclidean distance, Manhattan distance, or Dijkstra distance. Alternatively, a combination of two or more distance calculation methods can be used to determine the distance cost, which is not limited in this embodiment.

[0119] After precalculating the heuristic function values ​​corresponding to each alternative node position, the target alternative node position is determined from the set of alternative node positions based on the heuristic function values ​​of each alternative node position, and the heuristic function values ​​corresponding to each alternative node position are obtained; the heuristic function values ​​are compared, and the alternative node position with the smallest heuristic function value is determined as the target alternative node position, and the target alternative node position is updated to the current node position.

[0120] According to the method of this embodiment, the optimal target alternative node position is determined based on the heuristic function value of the alternative node position, that is, the current node position after each update is the optimal node position among the optimal alternative node positions, thereby improving the efficiency of determining the parking path of the tractor.

[0121] Based on the above embodiment, this embodiment further illustrates and optimizes the technical solution. Specifically, in this embodiment, a target candidate node position is determined from a set of candidate node positions, the target candidate node position is determined as the current node position, and the step of determining multiple node wheel angles corresponding to the current node position according to a preset sampling interval is returned; until a model node path of the bicycle model in the parking area is determined according to the current node position and the target parking node position, including:

[0122] Determine a target candidate node position from the candidate node position set, and determine the target candidate node position as the current node position;

[0123] If the current node position is the target parking node position, the model node path of the bicycle model in the parking area is determined based on the traversed current node positions and the target parking node position;

[0124] If the current node position is not the target parking node position, then determining whether a model node path can be determined within the parking area based on the current node position and the target parking node position;

[0125] If the model node path is not determined, the process returns to the step of determining a plurality of node wheel angles corresponding to the current node position according to a preset sampling interval.

[0126] Specifically, based on the correspondence between the semi-trailer system model and the single-vehicle model, the target parking node position corresponding to the target parking position is determined in advance; after the target candidate node position is determined from the set of candidate node positions and the target candidate node position is determined as the current node position, it is judged whether the current node position is the target parking node position; if the current node position is the target parking node position, it means that the target parking node position has been searched, so the model node path of the single-vehicle model in the parking area is determined based on the traversed current node positions and target parking node positions; specifically, the adjacent current node positions can be connected, and then the curve obtained by connection is smoothed to obtain the model node path.

[0127] If the current node position is not the target parking node position, a determination is made based on the current node position and the target parking node position as to whether a model node path can be determined within the parking area. That is, a determination is made as to whether a path curve can be generated based on the current node position to connect to the end position of the parking path, i.e., the target parking node position. The path curve may be a Reeds-Shepp curve or a Dubins curve, and this embodiment does not limit the curve type. If a path curve is determined, a model node path of the bicycle model within the parking area is determined based on the traversed current node positions and the path curve. Otherwise, a path curve cannot be determined based on the current node position and the target parking node position, indicating that a model node path cannot be determined at present. Therefore, the process returns to the step of determining multiple node wheel angles corresponding to the current node position based on a preset sampling interval, i.e., updating the current node position, and continuing the position search based on the current node position.

[0128] It can be seen that according to the method of this embodiment, the model node path can be determined promptly and accurately during iterative position search, thereby improving the efficiency of determining the parking path of the tractor and improving the efficiency of parking path planning.

[0129] Based on the above embodiment, this embodiment further illustrates and optimizes the technical solution. Specifically, in this embodiment, a model node path is determined within the parking area based on the current node position and the target parking node position, including:

[0130] Determining a first path curve according to the traversed current node positions;

[0131] Determine the polynomial expression of the helix;

[0132] Determine the state vector corresponding to the spiral line according to the node state change of the bicycle model;

[0133] Determining a parametric equation of the spiral according to the current node position, the target parking node position, and the state vector, and determining a second path curve within the parking area according to the parametric equation;

[0134] A model node path within the parking area is determined based on the first path curve and the second path curve.

[0135] In this embodiment, the model node path includes two parts: one part is a first path curve corresponding to the initial node position to the current node position, and the first path curve can be determined based on the traversed current node positions; the other part is a second path curve corresponding to the current node position to the target parking node position.

[0136] In this embodiment, a spiral line is used to connect the current node position and the target parking node position to determine whether a second path curve can be determined within the parking area. The polynomial expression for determining the spiral line is as follows:

[0137]

[0138] Among them, {a, b, c, d} are the coefficients in the polynomial expression, s t is the node state change of the bicycle model, that is, the arc length variable of the model node path corresponding to the rear axle center of the bicycle model, that is, the arc length variable of the trailer path at the rear axle center of the trailer; then the state vector corresponding to the spiral line is determined based on the node state change of the bicycle model, that is, the state vector of any position within the arc length range corresponding to the spiral line is determined as follows:

[0139]

[0140] Perform integral calculation based on the state vector to determine the expression of the solution of the differential equation and obtain the parametric equation of the second path curve:

[0141]

[0142]

[0143]

[0144] According to the current node position and the target parking node position, the starting state X0 (x0, y0, θ0, k0) and the end state X0 of the spiral are determined. des (x des ,y des ,θ des ,k des ); according to the starting state and the end state, determine the coefficients {a, b, c, d} in the polynomial expression of the spiral line, and obtain the parametric equation (expression) corresponding to the spiral line; the parametric equation (expression) of the spiral line represents the parametric equation of the model node path of the bicycle model, that is, the second path curve of the bicycle model; that is, the parametric equation of the trailer path at the center of the rear axle of the trailer.

[0145] It should be noted that if the parametric equation of the spiral line is determined based on the current node position and the target parking position, it means that a path curve can be determined within the parking area based on the current node position and the target parking node position. Therefore, on the basis of determining the first path curve based on the traversed current node positions, the model node path from the initial node position to the target parking node position is determined based on the first path curve and the second path curve, and the model node path of the bicycle model within the parking area is output; if the parametric equation of the spiral line cannot be obtained, it means that the path curve cannot be determined within the parking area based on the current node position and the target parking node position, and the model node path of the bicycle model within the parking area cannot be output, so the step of determining multiple node wheel angles corresponding to the current node position according to the preset sampling interval is returned.

[0146] It's understandable that when controlling a semitrailer for parking, as the tractor follows the parking path to the target parking position, the trailer's posture must also meet the vehicle posture requirements corresponding to the target parking position. Compared to the Reeds-Shepp and Dubins curves, the helix's curvature is continuous, facilitating the output of a model node path based on a single vehicle model.

[0147] It can be seen that according to the method of this embodiment, the curvature of the determined spiral line is continuous and conforms to the kinematic relationship of the semi-trailer. The spiral line is used to connect the current node position and the target parking node position. The coordinates and curvatures of the two ends of the spiral line are the constraints of the spiral line. Therefore, as long as the curvature of the starting position and the end position is determined, it can be ensured that the output path curve always meets the vehicle posture requirements of the semi-trailer.

[0148] Based on the above embodiment, this embodiment further illustrates and optimizes the technical solution. Specifically, in this embodiment, determining a model node path within a parking area based on the current node position and the target parking node position includes:

[0149] Determine the expressions of the first helix and the second helix;

[0150] Determine the state vectors of the first helix and the second helix according to the node state change of the bicycle model;

[0151] determining a first parametric equation of a first spiral according to the current node position, the initial node position, and the state vector, and determining a first path curve within the parking area according to the first parametric equation;

[0152] determining a second parametric equation of a second helix according to the current node position, the target parking node position, and the state vector, and determining a second path curve within the parking area according to the second parametric equation;

[0153] A model node path within the parking area is determined based on the first path curve and the second path curve.

[0154] In this embodiment, the model node path includes two parts: one part is a first path curve corresponding to the initial node position to the current node position; the other part is a second path curve corresponding to the current node position to the target parking node position.

[0155] Among them, this embodiment uses a first spiral line to connect the current node position and the initial node position to determine whether the first path curve can be determined within the parking area; and uses a second spiral line to connect the current node position and the target parking node position to determine whether the second path curve can be determined within the parking area.

[0156] Then, the state vectors of the first helix and the second helix are determined based on the node state change of the bicycle model, that is, the state vectors of any position within the arc length range corresponding to the first helix and the second helix are determined, and then the first parametric equation corresponding to the first helix and the second parametric equation corresponding to the second helix are determined respectively.

[0157] The starting point state and the end point state of the first spiral line are determined according to the current node position and the initial node position, and the coefficients in the polynomial expression of the first spiral line are determined according to the current node position and the initial node position to obtain the parametric equation (expression) of the first spiral line; the parametric equation (expression) corresponding to the first spiral line represents the path curve of the bicycle model from the initial node position to the current node position.

[0158] The starting and ending states of the second spiral are determined according to the current node position and the target parking node position, and the coefficients in the polynomial expression of the second spiral are determined according to the current node position and the target parking node position to obtain the parametric equation (expression) of the second spiral; the parametric equation (expression) corresponding to the second spiral represents the path curve of the bicycle model from the current node position to the target parking node position.

[0159] In this embodiment, a first curved path corresponding to the current node position from the current node position to the initial node position and a second curved path corresponding to the current node position to the target parking node position are determined based on the current node position. Since the first path curve and the second path curve are both spiral lines, the determined simulated path curve is a path curve with continuous curvature. The parking path of the tractor is a path curve with continuous curvature. Therefore, this method can ensure the feasibility of the determined parking path of the tractor.

[0160] It should be noted that when performing a position search based on the hybrid A* algorithm, each time the current node position is determined, multiple node wheel angles corresponding to the current node position are determined based on a preset sampling interval. Therefore, the node wheel angles corresponding to the current node positions determined twice adjacently may suddenly change, generating a step signal that causes the curvature of the model node path to be discontinuous. For the semi-trailer system model, the steering rate of the tractor's wheels can be adjusted, or even the semi-trailer can be made to travel along the parking path by turning in place. In this embodiment, the trailer of the semi-trailer is regarded as the body in the single-vehicle model, and the tractor of the semi-trailer is regarded as the wheel in the single-vehicle model. Therefore, after the model node path is determined based on the current node position and the target parking node position, it is necessary to further check the angle between the tractor and the trailer represented by each path point on the model node path. Whether the wheel turning angle θ of the tractor meets the driving safety requirements, ensure that the posture of the semi-trailer meets the kinematic relationship of the semi-trailer system model, and avoid the folding of the tractor and trailer.

[0161] Based on the above embodiment, this embodiment further illustrates and optimizes the technical solution. Specifically, in this embodiment, the parking path of the tractor is determined based on the model node path, the correspondence between the semi-trailer system model and the single vehicle model, and the kinematic relationship between the tractor and the trailer, including:

[0162] According to the corresponding relationship between the model node path and the semi-trailer system model and the single vehicle model, the angle between the tractor and the trailer and the wheel turning angle of the tractor corresponding to each path point on the model node path are determined;

[0163] If the included angle and the wheel turning angle of the tractor do not exceed the corresponding maximum angle threshold, a candidate parking path for the tractor is determined based on the model node path, and whether the candidate parking path of the tractor collides with an obstacle is determined;

[0164] If the candidate parking path of the tractor does not collide with an obstacle, the candidate parking path is determined as the parking path of the tractor;

[0165] If the angle or the wheel angle of the tractor exceeds the maximum angle threshold, and / or the candidate parking path of the tractor collides with an obstacle, the process returns to the step of determining the wheel angles of multiple nodes corresponding to the current node position according to a preset sampling interval.

[0166] Specifically, the model node path is the driving path of the single vehicle model. Each path point on the model node path is the path point of the trailer path corresponding to the center of the trailer's rear axle. The angle between the tractor and the trailer is determined according to the trailer posture corresponding to each path point on the model node path. and the wheel angle θ of the tractor.

[0167] According to the kinematic relationship corresponding to the semi-trailer system model, the motion speed v of the trailer rear axle center is determined t and the trailer's heading angle change

[0168]

[0169]

[0170] Where v represents the speed of the center of the rear axle of the tractor; Indicates the change in the heading angle of the semi-trailer; is the node wheel angle corresponding to the current node position of the single vehicle model, that is, the angle between the tractor and the trailer; L1 represents the wheelbase of the trailer; H is the center distance (x, y) from the hinge point of the tractor and trailer to the center of the rear axle of the tractor.

[0171] According to the kinematic relationship of the semi-trailer system model, the change in the heading angle of the tractor is determined as follows:

[0172]

[0173] According to the corresponding relationship between the semi-trailer system model and the single vehicle model, when searching for a position based on the current parking position, assuming that the wheel angle θ of the tractor is 0, then according to It can be seen that the change in the heading angle of the tractor is Also 0;

[0174] The corresponding movement speed v of the center of the rear axle of the trailer t for:

[0175]

[0176] Among them, v t Indicates the speed of the trailer's rear axle center;

[0177] Correspondingly, the change in the trailer's heading angle is for:

[0178]

[0179] Based on the kinematic relationship of the equivalent model, according to the change in the heading angle of the trailer The angle between the tractor and the trailer can be obtained

[0180]

[0181] According to the angle between the tractor and the trailer And the heading angle of the trailer corresponding to the current parking position Get the heading angle of the tractor corresponding to the current parking position Right now

[0182] According to the change of the heading angle of the tractor The wheel angle θ of the tractor at the current parking position can be obtained:

[0183]

[0184] The trailer path corresponding to the trailer can be determined:

[0185]

[0186] Among them, k represents the current parking space position corresponding to the current node position; k+1 represents the next parking space position corresponding to the current parking space position, that is, the motion state of the trailer determined based on the single-vehicle model also satisfies the kinematic relationship of the semi-trailer.

[0187] Specifically, after determining the included angle and the wheel turning angle of the tractor, maximum angle thresholds corresponding to the included angle and the wheel turning angle of the tractor are obtained. If neither the included angle nor the wheel turning angle of the tractor exceeds the corresponding maximum angle thresholds, collision detection is further performed on the model node path. Specifically, a candidate parking path for the tractor is determined based on the model node path, and whether the candidate parking path of the tractor collides with an obstacle is determined. If the candidate parking path of the tractor does not collide with an obstacle, it indicates that the single-vehicle model can park according to the model node path, that is, the semi-trailer can park according to the candidate parking path. Therefore, the parking path of the tractor is determined based on the candidate parking path. Otherwise, that is, if the included angle or the wheel turning angle of the tractor exceeds the corresponding maximum angle threshold, and / or the candidate parking path of the tractor collides with an obstacle, it indicates that the tractor cannot park according to the candidate parking path. Therefore, it is determined that the model node path cannot be determined within the parking area, and the position search needs to be continued. The process returns to the step of determining multiple node wheel turning angles corresponding to the current node position according to the preset sampling interval.

[0188] According to the method of this embodiment, after determining the model node path based on the current node position and the target parking node position, the vehicle states of the tractor and trailer in the semi-trailer system model are further determined based on the model node path, so that the determined parking path can ensure that the angle between the tractor and the trailer and the wheel angle of the tractor are within a reasonable range, and avoid collisions between the semi-trailer and obstacles in the parking area, thereby improving the effectiveness of the determined parking path for the tractor.

[0189] It is understandable that in practical applications, there are generally requirements for the semi-trailer's vehicle posture at the target parking position, and restrictions on the semi-trailer's vehicle posture during parking. That is, the requirements for the semi-trailer's vehicle posture at the target parking position are more stringent, while the vehicle posture during parking can be kept within a reasonable range, such as to avoid the tractor and trailer from collapsing. In the above embodiment, a spiral line is used to connect the current node position and the target parking node position, or a spiral line is used to connect the initial node position and the target parking node position. The expression of the spiral line can well constrain the curvature of the model node path. Based on the mathematical relationship between the path curvature and the vehicle turning angle in the kinematic model, when converting the model node path into the tractor's parking path, the angle between the tractor and trailer corresponding to the semi-trailer during parking and at the target parking position can also be constrained. Therefore, by constraining the starting and ending states of the spiral line, it can be ensured that the output parking path can always meet the requirements for the semi-trailer's vehicle posture.

[0190] In order to help those skilled in the art better understand the technical solutions in this application, the following describes the technical solutions in the embodiments of this application in detail in combination with actual application scenarios. In this embodiment of the application, the specific steps of a parking path planning method for a semi-trailer are as follows:

[0191] Step 1: Obtain the initial vehicle state of the semi-trailer, the target parking position, the parking area, and the obstacle information corresponding to the parking area; the initial vehicle state includes the initial parking position and the initial heading angle; the initial vehicle state includes the initial parking position and the initial heading angle;

[0192] Step 2: Initialize three node sets: Open_list set, Close_list set, and Open_pq set. The Open_list set is used to store all candidate node positions determined during the position search process, that is, the candidate node positions determined based on the current node position each time. The Close_list set is used to store traversed node positions, that is, the node positions that have been searched as the current node position. The Open_pq set is the candidate node position set, which is used to store the candidate node positions that have not been traversed in the Open_list set. That is, the corresponding set after the Close_list set is deleted from the Open_list set, and the candidate node positions in the Open_pq set are sorted according to certain rules.

[0193] Step 3: Convert the semi-trailer system model corresponding to the semi-trailer into an equivalent single vehicle model; where the center of the rear axle of the semi-trailer (x t ,y t ) is determined as the rear axle center of the single vehicle model, and the hinge point of the tractor and trailer of the semi-trailer is connected to the rear axle center of the trailer (xt ,y t ) is determined as the wheelbase of a single vehicle in the single vehicle model, and the tractor is determined as the wheel of a single vehicle in the single vehicle model;

[0194] Step 4: Determine the current node state of the single vehicle model based on the initial vehicle state of the semi-trailer system model; the current node state includes the current node position and the current node heading angle;

[0195] Step 5: Based on the current node position, obtain the maximum wheel angle constraint (-θ max ,θ max ), sampling is performed under the maximum wheel angle constraint according to the preset sampling interval to obtain the corresponding multiple node wheel angles

[0196]

[0197] Step 6: Calculate the wheel angle of each node corresponding to the current node position Determine a predicted node state of a next node position corresponding to the current node position based on the current node state of the current node position;

[0198] Step 7: Obtain a pre-set judgment rule and determine, based on the judgment rule, whether the predicted node state corresponding to the next node position meets the requirement for determining the next node position as a candidate node position:

[0199] Determine whether the next node position is a node position in the Open_list set based on the predicted node state; if the next node position is a node position in the Open_list set; if the next node position is not a node position in the Open_list set, determine whether the next node position collides with an obstacle in the parking area based on the predicted node state of the next node position; if the next node position collides with an obstacle in the parking area, ignore the next node position; if the next node position does not collide with an obstacle in the parking area, determine the next node position as an alternative node position, and store the alternative node position in the Open_list set and the Open_pq set (alternative node position set);

[0200] Step 8: Based on the heuristic function f, calculate the heuristic function value corresponding to the candidate node position, and store the candidate node position and the corresponding heuristic function value in the Open_list set and the Open_pq set (the candidate node position set). The heuristic function f includes a cost term g and a heuristic term h. The cost term g is usually set according to actual needs, such as an additional cost value such as a large turn or an excessively high turn rate, to achieve the purpose of preventing the semi-trailer from engaging in these behaviors. The heuristic term h is usually the cost value of the distance from the semi-trailer's current parking space to the target parking space, so that the semi-trailer continues to move closer to the target parking space.

[0201] Step 9: After traversing the wheel angles of each node at the current node position, obtain the updated Open_list set and Open_pq set (alternative node position set), and store the current node position in the Close_list set; wherein, the Open_pq set stores the labels corresponding to the alternative node positions and the heuristic function values ​​corresponding to the alternative node positions, and the labels of the alternative node positions are arranged from small to large according to the heuristic function values; the Open_list set stores the labels of the alternative node positions, the heuristic function values ​​corresponding to the alternative node positions, and the node states corresponding to the alternative node positions;

[0202] Step 10: Based on the heuristic function values ​​of each candidate node position, the candidate node position with the smallest heuristic function value in the Open_pq set (candidate node position set) is determined as the target candidate node position, and the target candidate node position is determined as the current node position; specifically, according to the arrangement order of the labels in the Open_pq set, the label at the top of the queue is selected as the target label, and the corresponding node state is extracted from the Open_list set according to the target label; then, the target candidate node position (updated current node position) is deleted from the Open_pq set (candidate node position set); if the current node position is the target parking node position, the model node path of the bicycle model in the parking area is determined based on the current node positions and the target parking node position; the target parking node position is the node position corresponding to the target parking position;

[0203] Step 11: If the current node position is not the target parking node position, determine the expressions of the first spiral and the second spiral; determine the state vectors of the first spiral and the second spiral based on the node state change of the bicycle model;

[0204] Step 12: Determine a first parametric equation for the first spiral based on the current node position, the initial node position, and the state vector, and determine a first path curve within the parking area based on the first parametric equation; determine a second parametric equation for the second spiral based on the current node position, the target parking node position, and the state vector, and determine a second path curve within the parking area based on the second parametric equation; and determine a model node path within the parking area based on the first path curve and the second path curve.

[0205] Step 13: According to the corresponding relationship between the model node path and the semi-trailer system model and the single vehicle model, determine the angle between the tractor and the trailer corresponding to each path point on the model node path and the wheel angle θ of the tractor;

[0206] Step 14: If the angle between the tractor and the trailer If the wheel angle θ of the tractor and the wheel angle θ of the tractor do not exceed the corresponding maximum angle threshold, the candidate parking path of the tractor is determined according to the model node path, and whether the candidate parking path of the tractor collides with the obstacle is determined;

[0207] Step 15: If the candidate parking path of the tractor does not collide with an obstacle, the candidate parking path is determined as the parking path of the tractor;

[0208] Step 16: If the angle between the tractor and the trailer If the wheel turning angle θ of the tractor exceeds the maximum angle threshold, and / or the candidate parking path of the tractor collides with an obstacle, it is determined that the path curve cannot be determined within the parking area, and the process returns to step 5: Based on the current node position, the maximum wheel turning angle constraint (-θ max ,θ max ), sampling is performed under the maximum wheel angle constraint according to the preset sampling interval to obtain the corresponding multiple node wheel angles steps.

[0209] The present invention provides a method for planning a parking path for a semitrailer. The method converts the semitrailer system model corresponding to the semitrailer into an equivalent single-vehicle model, considers the semitrailer's trailer as the vehicle body in the single-vehicle model, and considers the semitrailer's tractor as the wheel in the single-vehicle model. The method then uses the initial vehicle state and the target parking position to determine the model node path of the single-vehicle model within the parking area based on a hybrid A* algorithm. In other words, when determining the parking path for the tractor, the method first determines the model node path based on the single-vehicle model, and then determines the parking path for the tractor based on the model node path, the correspondence between the semitrailer system model and the single-vehicle model, and the kinematic relationship between the tractor and the trailer. Therefore, when the tractor parks according to the parking path, it can also take into account the vehicle posture of the trailer, ensuring parking safety. Furthermore, converting the semitrailer system model corresponding to the semitrailer into an equivalent single-vehicle model can reduce the vehicle state dimension when performing position search using the hybrid A* algorithm, thereby improving the efficiency of parking path planning.

[0210] It should be understood that the size of the serial numbers of the steps in the above embodiments 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 embodiments of this application.

[0211] Figure 7 FIG. 1 is a schematic diagram of a parking path planning device for a semi-trailer provided in an embodiment of the present application. Figure 7 As shown, the parking path planning device for a semi-trailer in this embodiment includes an acquisition module 710, an equivalent module 720, a position search module 730 and a path determination module 740; wherein,

[0212] The acquisition module 710 is used to obtain the initial vehicle state, target parking position and parking area of ​​the semi-trailer; the initial vehicle state includes the initial parking position and the initial heading angle;

[0213] The equivalent module 720 is configured to convert the semitrailer system model corresponding to the semitrailer into an equivalent single-vehicle model; wherein the center of the rear axle of the semitrailer is determined as the center of the rear axle of the single-vehicle model, the center distance from the hinge point of the tractor and trailer of the semitrailer to the center of the rear axle of the trailer is determined as the wheelbase of the single-vehicle model, and the tractor is determined as a single wheel of the single-vehicle model;

[0214] A position search module 730 is configured to determine a model node path of a single vehicle model within the parking area based on a hybrid A* algorithm using the initial vehicle state and the target parking position;

[0215] The path determination module 740 is used to determine the parking path of the tractor according to the model node path, the correspondence between the semi-trailer system model and the single vehicle model, and the kinematic relationship between the tractor and the trailer.

[0216] The parking path planning device for a semi-trailer provided in an embodiment of the present application has the same beneficial effects as the above-mentioned parking path planning method for a semi-trailer.

[0217] In one embodiment, the location search module includes:

[0218] The node state determination submodule is used to determine the current node state of the single vehicle model according to the initial vehicle state of the semi-trailer system model; the current node state includes the current node position and the current node heading angle;

[0219] A node wheel angle determination submodule is used to determine a plurality of node wheel angles corresponding to the current node position according to a preset sampling interval;

[0220] A predicted state determination submodule is used to determine, for each node wheel angle corresponding to the current node position, a predicted node state of a next node position corresponding to the current node position according to the current node state of the current node position;

[0221] The candidate node determination submodule is used to determine whether to determine the next node position as a candidate node position according to the predicted node state, and add the candidate node position to the candidate node position set;

[0222] A position set determination submodule is used to obtain an updated candidate node position set after traversing the wheel angles of each node at the current node position;

[0223] The position update submodule is used to determine the target alternative node position from the set of alternative node positions, determine the target alternative node position as the current node position, and call the node wheel angle determination submodule until the model node path of the bicycle model in the parking area is determined based on the current node position and the target parking node position; the target parking node position is the node position corresponding to the target parking position determined based on the correspondence between the semi-trailer system model and the bicycle model.

[0224] In one embodiment, the candidate node determination submodule includes:

[0225] a condition judgment unit, configured to determine whether the next node position is a traversed node position, and to determine whether the next node position collides with an obstacle in the parking area based on the predicted node state and obstacle information;

[0226] The candidate node determination unit is used to determine the next node position as a candidate node position if the next node position is not a traversed node position and the next node position does not collide with an obstacle, and add the candidate node position to the candidate node position set.

[0227] In one embodiment, the location update submodule includes:

[0228] A heuristic function value determination unit, used to obtain the heuristic function value corresponding to each candidate node position;

[0229] The first position updating unit is configured to determine a target candidate node position from the candidate node position set according to each heuristic function value, and determine the target candidate node position as the current node position.

[0230] In one embodiment, the location update submodule includes:

[0231] A second position updating unit is configured to determine a target candidate node position from the candidate node position set, and determine the target candidate node position as the current node position;

[0232] a node path determining unit, configured to determine a model node path of the bicycle model within the parking area based on the traversed current node positions and the target parking node position if the current node position is the target parking node position;

[0233] The model node path determination unit is used to determine whether a model node path can be determined within the parking area based on the current node position and the target parking node position if the current node position is not the target parking node position; if the model node path is not determined, call the node wheel angle determination submodule.

[0234] In one embodiment, the model node path determination unit includes:

[0235] A first path curve determining subunit, configured to determine a first path curve according to the traversed current node positions;

[0236] A first expression determination subunit is used to determine a polynomial expression of a helix;

[0237] A first state vector determination subunit is used to determine the state vector corresponding to the spiral line according to the node state change of the bicycle model;

[0238] a first path curve determining subunit, configured to determine a parametric equation of the helix according to the current node position, the target parking node position, and the state vector, and to determine a second path curve within the parking area according to the parametric equation;

[0239] The first model node path determining subunit is configured to determine a model node path within the parking area according to the first path curve and the second path curve.

[0240] In one embodiment, the model node path determination unit includes:

[0241] a second expression determination subunit, configured to determine expressions of the first helix and the second helix;

[0242] A second state vector determining subunit, configured to determine the state vectors of the first helix and the second helix according to the node state variation of the bicycle model;

[0243] a second path curve determining subunit, configured to determine a first parametric equation of a first helix according to a current node position, an initial node position, and a state vector, and to determine a first path curve within the parking area according to the first parametric equation;

[0244] a third path curve determining subunit, configured to determine a second parametric equation of a second helix according to the current node position, the target parking node position, and the state vector, and to determine a second path curve within the parking area according to the second parametric equation;

[0245] The second model node path determining subunit is configured to determine a model node path within the parking area according to the first path curve and the second path curve.

[0246] In one embodiment, the path determination module 740 includes:

[0247] A parameter determination subunit is used to determine the angle between the tractor and the trailer and the wheel angle of the tractor corresponding to each path point on the model node path according to the corresponding relationship between the model node path and the semi-trailer system model and the single vehicle model;

[0248] a collision judgment subunit, configured to determine a candidate parking path for the tractor based on the model node path if the included angle and the wheel turning angle of the tractor do not exceed corresponding maximum angle thresholds, and determine whether the candidate parking path for the tractor collides with an obstacle;

[0249] The parking path determination subunit is configured to determine the candidate parking path as the tractor's parking path if the candidate parking path does not collide with an obstacle; and to invoke the node wheel angle determination submodule if the angle or the tractor's wheel angle exceeds a maximum angle threshold and / or the candidate parking path of the tractor collides with an obstacle.

[0250] It should be noted that the information interaction, execution process, etc. between the above-mentioned devices / units are based on the same concept as the method embodiment of this application. Their specific functions and technical effects can be found in the method embodiment section and will not be repeated here.

[0251] Those skilled in the art can clearly understand that, for the convenience and brevity of description, only the division of the above-mentioned functional units and modules is used as an example for illustration. In actual applications, the above-mentioned functions can be distributed and completed by different functional units and modules as needed, that is, the internal structure of the device can be divided into different functional units or modules to complete all or part of the functions described above. The functional units and modules in the embodiment can be integrated into one processing unit, or each unit can exist physically alone, or two or more units can be integrated into one unit. The above-mentioned integrated unit can be implemented in the form of hardware or in the form of software functional units. In addition, the specific names of the functional units and modules are only for the convenience of distinguishing each other, and are not used to limit the scope of protection of this application. The specific working process of the units and modules in the above-mentioned system can refer to the corresponding process in the aforementioned method embodiment, and will not be repeated here.

[0252] Figure 8 This is a schematic diagram of the structure of a terminal device provided in an embodiment of the present application. Figure 8 As shown, the terminal device 800 of this embodiment includes a memory 801, a processor 802, and a computer program 803 stored in the memory 801 and executable on the processor 802; when the processor 802 executes the computer program 803, the steps in the above-mentioned embodiments of the parking path planning method for the semi-trailer are implemented, such as the figure; or when the processor 802 executes the computer program 803, the functions of the modules / units in the above-mentioned embodiments of the device are implemented.

[0253] Exemplarily, the computer program 803 may be divided into one or more modules / units, one or more of which are stored in the memory 801 and executed by the processor 802 to implement the method of the embodiment of the present application. One or more modules / units may be a series of computer program instruction segments capable of performing specific functions, and the instruction segments are used to describe the execution process of the computer program 803 in the terminal device 800. For example, the computer program 803 may be divided into an acquisition module, an acquisition module, a location search module, and a path determination module, and the specific functions of each module are as follows:

[0254] An acquisition module is used to obtain the initial vehicle state, target parking position and parking area of ​​the semi-trailer; the initial vehicle state includes the initial parking position and initial heading angle;

[0255] An acquisition module is used to convert the semi-trailer system model corresponding to the semi-trailer into an equivalent single-vehicle model; wherein the center of the trailer rear axle of the semi-trailer is determined as the rear axle center of the single-vehicle model, the center distance from the hinge point of the tractor and trailer of the semi-trailer to the center of the trailer rear axle is determined as the single-vehicle wheelbase in the single-vehicle model, and the tractor is determined as a single wheel of the single-vehicle model;

[0256] A location search module is used to determine the model node path of the single vehicle model within the parking area based on the hybrid A* algorithm using the initial vehicle state and the target parking position;

[0257] The path determination module is used to determine the parking path of the tractor based on the model node path, the correspondence between the semi-trailer system model and the single vehicle model, and the kinematic relationship between the tractor and the trailer.

[0258] In application, the terminal device 800 can be a computing device such as a desktop computer, a notebook, a PDA, or a cloud server. The terminal device 800 can include but is not limited to a memory 801 and a processor 802. Those skilled in the art will understand that Figure 8 It is only an example of a terminal device and does not constitute a limitation of the terminal device. It may include more or fewer components than shown in the figure, or a combination of certain components, or different components. For example, the terminal device may also include input and output devices, network access devices, buses, etc.; among them, the input and output devices may include cameras, audio acquisition / playback devices, display screens, etc.; the network access device may include a communication module for wireless communication with external devices.

[0259] In applications, the processor may be a central processing unit (CPU), other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor or any conventional processor, etc.

[0260] In applications, memory can be an internal storage unit of a terminal device, such as a hard drive or memory; it can also be an external storage device, such as a plug-in hard drive, a Smart Media Card (SMC), a Secure Digital (SD) card, or a flash memory card. It can also include both internal and external storage units. Memory is used to store operating systems, applications, boot loaders, data, and other programs, such as computer program code. Memory can also be used to temporarily store data that has been output or is about to be output.

[0261] An embodiment of the present application further provides a computer-readable storage medium, which stores a computer program. When the computer program is executed by a processor, the steps in the above-mentioned method embodiments can be implemented.

[0262] A computer-readable storage medium provided in an embodiment of the present application has the same beneficial effects as the above-mentioned method for planning a parking path for a semi-trailer.

[0263] The present application implements all or part of the processes in the above-mentioned embodiment method, which can be completed by instructing the relevant hardware through a computer program. The computer program can be stored in a computer-readable storage medium. When the computer program is executed by the processor, it can implement the steps of the above-mentioned various method embodiments. The computer program includes computer program code, which can be in source code form, object code form, executable file or some intermediate form. The computer-readable medium may at least include: any entity or device capable of carrying the computer program code to a terminal device, a recording medium, a computer memory, a read-only memory (ROM), a random access memory (RAM), an electric carrier signal, a telecommunication signal and a software distribution medium. For example, a USB flash drive, a mobile hard disk, a magnetic disk or an optical disk.

[0264] In the above embodiments, the description of each embodiment has its own focus. For parts that are not described or recorded in detail in a certain embodiment, reference can be made to the relevant description of other embodiments.

[0265] Those skilled in the art will appreciate that the devices and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this application.

[0266] In the embodiments provided herein, it should be understood that the disclosed devices and methods may be implemented in other ways. For example, the device embodiments described above are merely illustrative. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be through some interface, or the devices may be indirectly coupled or communicated in some manner, whether electrical, mechanical, or other.

[0267] The above-described embodiments are only used to illustrate the technical solutions of the present application, rather than to limit them. Although the present application 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. 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 application, and should all be included in the scope of protection of the present application.

Claims

1. A parking path planning method for a semi-trailer, characterized in that: The method comprises: Obtaining an initial vehicle state, a target parking position, and a parking area of ​​the semi-trailer; the initial vehicle state includes an initial parking position and an initial heading angle; Converting the semitrailer system model corresponding to the semitrailer into an equivalent single-vehicle model; wherein the center of the rear axle of the semitrailer is determined as the rear axle center of the single-vehicle model, the center distance from the hinge point of the tractor of the semitrailer and the trailer to the center of the rear axle of the trailer is determined as the single-vehicle wheelbase in the single-vehicle model, and the tractor is determined as the single wheel of the single-vehicle model; Determining a model node path of the bicycle model within the parking area based on a hybrid A* algorithm using the initial vehicle state and the target parking position; The parking path of the tractor is determined according to the model node path, the corresponding relationship between the semi-trailer system model and the single vehicle model, and the kinematic relationship between the tractor and the trailer.

2. The method according to claim 1, characterized in that The method of determining a model node path of the bicycle model within the parking area based on a hybrid A* algorithm using the initial vehicle state and the target parking space position includes: Determining a current node state of the single vehicle model according to the initial vehicle state of the semi-trailer system model; the current node state includes a current node position and a current node heading angle; Determine a plurality of node wheel angles corresponding to the current node position according to a preset sampling interval; For each of the node wheel angles corresponding to the current node position, determining a predicted node state of a next node position corresponding to the current node position according to the current node state of the current node position; Determining whether to determine the next node position as a candidate node position according to the predicted node state, and adding the candidate node position to a set of candidate node positions; After traversing the wheel angles of each node at the current node position, an updated set of candidate node positions is obtained; Determine a target alternative node position from the set of alternative node positions, determine the target alternative node position as the current node position, and return to the step of determining multiple node wheel angles corresponding to the current node position according to a preset sampling interval; until the model node path of the bicycle model in the parking area is determined according to the current node position and the target parking node position; the target parking node position is a node position corresponding to the target parking position determined based on the correspondence between the semi-trailer system model and the bicycle model.

3. The method according to claim 2, characterized in that The determining, according to the predicted node state, whether to determine the next node position as a candidate node position, and adding the candidate node position to a set of candidate node positions, includes: determining whether the next node position is a traversed node position, and determining whether the next node position collides with an obstacle in the parking area based on the predicted node state and obstacle information; If the next node position is not the traversed node position and the next node position does not collide with the obstacle, the next node position is determined as the candidate node position, and the candidate node position is added to the candidate node position set.

4. The method according to claim 2, characterized in that The determining a target candidate node position from the candidate node position set and determining the target candidate node position as the current node position includes: Obtaining the heuristic function values ​​corresponding to the positions of the candidate nodes; A target candidate node position is determined from the candidate node position set according to each of the heuristic function values, and the target candidate node position is determined as the current node position.

5. The method according to any one of claims 2 to 4, characterized in that The step of determining a target candidate node position from the candidate node position set, determining the target candidate node position as the current node position, and returning to the step of determining a plurality of node wheel angles corresponding to the current node position according to a preset sampling interval; until a model node path of the bicycle model within the parking area is determined according to the current node position and the target parking node position, includes: Determine a target candidate node position from the candidate node position set, and determine the target candidate node position as the current node position; If the current node position is the target parking node position, determining a model node path of the bicycle model in the parking area according to the traversed current node positions and the target parking node position; If the current node position is not the target parking node position, determining whether a model node path can be determined within the parking area based on the current node position and the target parking node position; If the model node path is not determined, the process returns to the step of determining a plurality of node wheel angles corresponding to the current node position according to a preset sampling interval.

6. The method according to claim 5, characterized in that Determining a model node path within the parking area according to the current node position and the target parking node position includes: Determining a first path curve according to the traversed current node positions; Determine the polynomial expression of the helix; Determining a state vector corresponding to the spiral line according to a node state change of the bicycle model; determining a parametric equation of the spiral according to the current node position, the target parking node position, and the state vector, and determining a second path curve within the parking area according to the parametric equation; A model node path within the parking area is determined according to the first path curve and the second path curve.

7. The method according to claim 5, characterized in that Determining a model node path within the parking area according to the current node position and the target parking node position includes: Determine the expressions of the first helix and the second helix; Determining the state vectors of the first helix and the second helix according to the node state change of the bicycle model; determining a first parametric equation of the first spiral according to the current node position, the initial node position, and the state vector, and determining a first path curve within the parking area according to the first parametric equation; determining a second parametric equation of the second spiral according to the current node position, the target parking node position, and the state vector, and determining a second path curve within the parking area according to the second parametric equation; A model node path within the parking area is determined according to the first path curve and the second path curve.

8. The method according to claim 6, characterized in that The determining of the parking path of the tractor according to the model node path, the correspondence between the semi-trailer system model and the single vehicle model, and the kinematic relationship between the tractor and the trailer includes: Determining the angle between the tractor and the trailer and the wheel angle of the tractor corresponding to each path point on the model node path according to the correspondence between the model node path and the semi-trailer system model and the single vehicle model; If neither the included angle nor the wheel turning angle of the tractor exceeds a corresponding maximum angle threshold, determining a candidate parking path for the tractor based on the model node path, and determining whether the candidate parking path for the tractor collides with an obstacle; If the candidate parking path of the tractor does not collide with the obstacle, determining the candidate parking path as the parking path of the tractor; If the included angle or the wheel angle of the tractor exceeds the maximum angle threshold, and / or the candidate parking path of the tractor collides with the obstacle, the process returns to the step of determining the wheel angles of multiple nodes corresponding to the current node position according to the preset sampling interval.

9. A parking path planning device for a semi-trailer, characterized in that: The device comprises: An acquisition module is used to acquire the initial vehicle state, target parking position and parking area of ​​the semi-trailer; the initial vehicle state includes the initial parking position and the initial heading angle; an equivalent module, configured to convert the semitrailer system model corresponding to the semitrailer into an equivalent single-vehicle model; wherein the center of the rear axle of the semitrailer is determined as the rear axle center of the single-vehicle model, the center distance from the hinge point of the tractor of the semitrailer and the trailer to the center of the rear axle of the trailer is determined as the single-vehicle wheelbase in the single-vehicle model, and the tractor is determined as the single wheel of the single-vehicle model; a position search module, configured to determine a model node path of the bicycle model within the parking area based on a hybrid A* algorithm using the initial vehicle state and the target parking position; A path determination module is used to determine the parking path of the tractor according to the model node path, the corresponding relationship between the semi-trailer system model and the single vehicle model, and the kinematic relationship between the tractor and the trailer.

10. A terminal device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein: When the processor executes the computer program, the steps of the method according to any one of claims 1 to 8 are implemented.

Images