Track planning and tracking control method of articulated vehicle and related device thereof

Abstract

The invention discloses a trajectory planning and tracking control method of an articulated vehicle and a related device thereof. The method comprises the following steps: searching a first planning trajectory of a tractor by using a heuristic search algorithm; for each piece of first track point information in the first planning track, determining second track point information corresponding to the first track point information according to the first track point information, a non-time reference dynamic equation about a target included angle between the tractor and the semitrailer and a geometrical relationship between the two vehicles, generating a second planned trajectory of the semitrailer according to the second trajectory point information; solving the path tracking control model of the semitrailer by taking the second planned trajectory as an expected trajectory of the semitrailer, and taking an optimal solution of the path tracking control model of the semitrailer as a tracking control quantity of the semitrailer; and solving the tractor path tracking control model by taking the first planned trajectory as an expected trajectory of the tractor, and taking an optimal solution of the tractor path tracking control model as a tracking control quantity of the tractor.

Description

Technical Field

[0001] This application relates to the field of intelligent driving technology, and more specifically, to a trajectory planning and tracking control method and related apparatus for articulated vehicles. Background Technology

[0002] Trailer caravans have become an integral part of leisure travel and even daily life for some. As a typical example of articulated vehicles, they offer convenience and diversity for leisure travel. However, they also pose significant challenges for inexperienced or technically challenged drivers to safely, accurately, and quickly complete driving operations, especially parking, in complex road environments. Therefore, researching and developing autonomous driving systems for articulated vehicles to address the difficulty of driving articulated vehicles for inexperienced or technically inexperienced drivers is a work of practical significance. Since autonomous driving systems for articulated vehicles need to meet the requirements of autonomous vehicles in terms of safety, comfort, feasible parking space, parking accuracy, and real-time performance, this presents significant challenges to the design and implementation of trajectory planning and tracking control methods for autonomous driving systems of articulated vehicles. Summary of the Invention

[0003] This application provides a trajectory planning and tracking control method and related device for articulated vehicles, which can realize the autonomous driving function of articulated vehicles while meeting requirements such as safety and comfort.

[0004] The specific technical solution is as follows:

[0005] In a first aspect, embodiments of this application provide a trajectory planning and tracking control method for an articulated vehicle, the articulated vehicle comprising a tractor and a semi-trailer articulated with the tractor, the method comprising:

[0006] The first planned trajectory of the tractor is searched using a heuristic search algorithm;

[0007] For each first trajectory point in the first planned trajectory, based on the first trajectory point information, the non-time reference dynamic equation regarding the target angle between the tractor and the semi-trailer, and the geometric relationship between the tractor and the semi-trailer, the second trajectory point information corresponding to the first trajectory point information is determined, and the second planned trajectory of the semi-trailer is generated based on multiple second trajectory point information. The non-time reference dynamic equation is a discretized equation with the target angle as the state variable and the front wheel rotation angle of the tractor as the control variable, and the second trajectory point information is the trajectory point information of the semi-trailer.

[0008] The semi-trailer path tracking control model is solved by taking the second planned trajectory as the desired trajectory of the semi-trailer, and the optimal solution of the semi-trailer path tracking control model is taken as the tracking control quantity of the semi-trailer. The semi-trailer path tracking control model is a path tracking control model with the optimization objective of minimizing the weighted average tracking error of the semi-trailer lateral tracking error, the tracking error of the semi-trailer yaw angle, and the tracking error of the target angle between the tractor and the semi-trailer.

[0009] The tractor path tracking control model is solved by taking the first planned trajectory as the desired trajectory of the tractor, and the optimal solution of the tractor path tracking control model is taken as the tracking control quantity of the tractor. The tractor path tracking control model is a path tracking control model with the optimization objective of minimizing the weighted average tracking error of the tractor lateral tracking error, the tracking error of the tractor yaw angle, and the tracking error of the tractor front wheel steering angle.

[0010] As can be seen from the above scheme, the embodiments of this application can first plan the curvature-continuous tractor trajectory and semi-trailer trajectory based on the heuristic search algorithm, the non-time reference dynamic equation of the target angle between the tractor and the semi-trailer, and the geometric relationship between the tractor and the semi-trailer. Then, using a path tracking control model with the optimization objective of minimizing the weighted average of the semi-trailer's lateral tracking error, semi-trailer's yaw angle tracking error, and the target angle tracking error between the tractor and the semi-trailer, the semi-trailer is tracked and controlled. Using a path tracking control model with the optimization objective of minimizing the weighted average of the tractor's lateral tracking error, tractor's yaw angle tracking error, and tractor's front wheel steering angle tracking error, the tractor is tracked and controlled. Thus, the automatic driving function of the articulated vehicle can be realized while meeting the requirements of safety and comfort.

[0011] In one possible implementation, the basic action units of the heuristic search algorithm include a spiral basic action unit, a circular arc basic action unit, and a straight line segment basic action unit.

[0012] The basic spiral motion unit includes a spiral line from a straight line segment to a spiral line with a first preset radius, a spiral line from a first target radius to a second target radius, a spiral line from a second target preset radius to a first target radius, and a spiral line from the first preset radius to a straight line segment. The first target radius and the second target radius are two adjacent preset radii among a plurality of preset radii set in a preset order, and the first target radius includes the first preset radius.

[0013] The basic arc motion unit includes an arc with radii of each of the plurality of preset radii, which extends toward the target direction;

[0014] The angular change is the same for each basic spiral motion unit and each basic circular arc motion unit.

[0015] The process of searching the first planned trajectory of the tractor using a heuristic search algorithm includes:

[0016] Based on the principle of continuous curvature of the planned trajectory, the planned trajectory of the tractor is obtained by heuristically searching by splicing the basic action units of the spiral, the basic action units of the circular arc and the basic action units of the straight line segment.

[0017] As can be seen from the above scheme, the embodiments of this application can add a spiral basic motion unit on the basis of circular arc and straight line segments, and splice the three basic motion units according to the principle of continuous curvature of the planned trajectory to realize the search of trajectory planning, thereby improving the success rate of trajectory planning, space utilization, and trajectory quality that is friendly to tracking and control. In addition, by setting the angle change of each spiral basic motion unit and circular arc basic motion unit to be the same, it is convenient to use a three-dimensional array to store and look up the occupied grid.

[0018] In one possible implementation, the step of searching the first planned trajectory of the tractor using a heuristic search algorithm includes:

[0019] The heuristic search algorithm is used to search for the first trajectory point information of the tractor at time k+1;

[0020] The target angle at time k+1 is calculated based on the target angle at time k, the front wheel angle of the tractor at time k, the distance from the midpoint of the rear axle of the tractor to the articulation point, the distance from the articulation point to the midpoint of the rear axle of the semi-trailer, the distance from the midpoint of the front axle of the tractor to the midpoint of the rear axle, the first calculation step size, and the non-time reference dynamic equation.

[0021] When the target angle at time k+1 is greater than or equal to a preset angle threshold, the heuristic search algorithm is used again to search for the first trajectory point information of the tractor at time k+1.

[0022] As can be seen from the above scheme, the embodiments of this application can not only calculate the target angle at each planning moment based on the non-time reference dynamic equation, but also filter invalid trajectory search results through a preset angle threshold, thereby avoiding the tractor and semi-trailer from folding and becoming unstable, and keeping the tractor and semi-trailer in a steady circular motion.

[0023] In one possible implementation, the preset included angle threshold is determined based on the distance from the midpoint of the tractor's rear axle to the hinge point, the distance from the hinge point to the midpoint of the semi-trailer's rear axle, and the minimum turning radius of the tractor.

[0024] In one possible implementation, the preset included angle threshold includes:

[0025]

[0026] Wherein, the β max The preset included angle threshold is represented by M1, which represents the distance from the midpoint of the rear axle of the tractor to the articulation point, and L2 represents the distance from the articulation point to the midpoint of the rear axle of the semi-trailer. R... min This indicates the minimum turning radius of the tractor unit.

[0027] In one possible implementation, determining second trajectory point information corresponding to the first trajectory point information based on the first trajectory point information, a non-time-referenced dynamic equation regarding the target angle between the tractor and the semi-trailer, and the geometric relationship between the tractor and the semi-trailer includes:

[0028] The target angle at time k+1 is calculated based on the target angle at time k, the front wheel angle of the tractor at time k, the distance from the midpoint of the rear axle of the tractor to the articulation point, the distance from the articulation point to the midpoint of the rear axle of the semi-trailer, the distance from the midpoint of the front axle of the tractor to the midpoint of the rear axle, the first calculation step size, and the non-time reference dynamic equation.

[0029] For the same target time, based on the planned pose of the tractor at the target time and the geometric relationship between the tractor and the semi-trailer in the first trajectory point information, the planned pose of the semi-trailer at the target time is calculated. Based on other state information of the tractor at the target time and the geometric relationship between the tractor and the semi-trailer in the first trajectory point information, other state information of the semi-trailer at the target time is calculated. The other state information includes at least the planned vehicle speed.

[0030] In one possible implementation, the non-time-referenced dynamic equations include:

[0031] β k+1 =β k +hK2

[0032] in,

[0033]

[0034] Wherein, the β k+1 β k Let K1 and K2 represent the target angles at time (k+1) and time (k), respectively; let h represent the first calculation step size; let K1 and K2 represent the coefficients of the second-order Runge-Kutta integral method; and let δ... kL1 represents the turning angle of the front wheel of the tractor at time k, L2 represents the distance from the center of the front axle to the center of the rear axle of the tractor, M1 represents the distance from the articulation point to the midpoint of the rear axle of the semi-trailer, and M2 represents the distance from the midpoint of the rear axle of the tractor to the articulation point.

[0035] In one possible implementation, the method further includes:

[0036] Using the geometric relationship between the speeds of the tractor and the semi-trailer, and the planned speed of the tractor included in the first planned trajectory, the temporary planned speed of the semi-trailer is calculated.

[0037] The predicted trajectory length of the semi-trailer is deduced based on the temporary planned speed and planning time of the semi-trailer.

[0038] Using the association model of the tractor and the semi-trailer, the predicted trajectory length of the semi-trailer is converted into the predicted trajectory length of the tractor.

[0039] Based on the predicted trajectory length of the tractor, the planned speed of the tractor in the first planned trajectory is corrected to obtain the corrected planned speed of the tractor.

[0040] As can be seen from the above scheme, after obtaining the first planned trajectory of the tractor using a heuristic search algorithm, the embodiment of this application can reverse-engineer a more reasonable and reliable planned speed that the tractor should have by using the geometric relationship between the speeds of the tractor and the semi-trailer and the association model of the tractor and the semi-trailer, and then correct the original planned speed based on the reverse-engineered planned speed.

[0041] In one possible implementation, the method further includes:

[0042] The planned speed of the tractor is adjusted using the sliding window method so that the planned speed of the tractor changes in advance.

[0043] The planned vehicle speed after the sliding window method is smoothed by a constraint optimization method, and the smoothed planned vehicle speed of the tractor is obtained as the final planned vehicle speed.

[0044] Using the geometric relationship between the speeds of the tractor and the semi-trailer, and the smoothed planned speed of the tractor, the smoothed planned speed of the semi-trailer is calculated as the final planned speed.

[0045] As can be seen from the above scheme, the embodiments of this application can further use the sliding window method and constraint optimization method to smooth the corrected planned speed on the basis of correcting the planned speed of the tractor. Based on the geometric relationship between the speeds of the tractor and the semi-trailer, the planned speed of the semi-trailer can be adaptively adjusted, thereby suppressing frequent fluctuations in the maximum allowable speed corresponding to the planned trajectory, and thus improving the safety and comfort of vehicle driving.

[0046] In one possible implementation, the semi-trailer path tracking control model includes:

[0047]

[0048] Wherein, the ω f (k) represents the front wheel angular velocity of the semi-trailer to be tracked and controlled at time k, N represents time N, y2(k) and y2(k+1) represent the lateral position coordinates of the semi-trailer to be tracked and controlled at time k and time k+1, respectively, x2(k) and x2(k+1) represent the longitudinal position coordinates of the semi-trailer to be tracked and controlled at time k and time k+1, respectively, θ2(k) and θ2(k+1) represent the yaw angles of the semi-trailer to be tracked and controlled at time k and time k+1, respectively, and β k The y represents the included angle of the target to be tracked and controlled at time k. 2,ref (k) represents the expected lateral position coordinate of the semi-trailer at time k, where θ 2,ref (k) represents the expected yaw angle of the semi-trailer at time k, and β ref (k) represents the expected target angle of the semi-trailer at time k, Q2 represents the weight coefficient for the process points of the semi-trailer's planned trajectory, W2 represents the weight coefficient for the endpoint of the semi-trailer's planned trajectory, v2 represents the planned speed of the semi-trailer, Δt represents the second calculation step size, L2 represents the distance from the articulation point to the midpoint of the semi-trailer's rear axle, and β max This indicates the preset included angle threshold.

[0049] As can be seen from the above scheme, the semi-trailer path tracking control model provided in this application can improve the stability of the entire system by introducing a penalty term for the end state of the planned trajectory in the optimization objective. Furthermore, considering the target angle tracking error between the tractor and the semi-trailer in the optimization objective is equivalent to introducing a feedforward control quantity in the semi-trailer model predictive control, which can improve the stability and convergence speed of the entire system.

[0050] In one possible implementation, the tractor path tracking control model includes:

[0051]

[0052] Wherein, δ(k) represents the front wheel steering angle of the tractor to be tracked and controlled at time k, N represents time N, y1(k) and y1(k+1) represent the lateral position coordinates of the tractor to be tracked and controlled at time k and time k+1, respectively, x1(k) and x1(k+1) represent the longitudinal position coordinates of the tractor to be tracked and controlled at time k and time k+1, respectively, θ1(k) and θ1(k+1) represent the yaw angle of the tractor to be tracked and controlled at time k and time k+1, respectively. 1,ref (k) represents the desired lateral position coordinate of the tractor at time k, where θ 1,ref (k) represents the expected yaw angle of the tractor at time k, and the δ ref (k) represents the desired target angle of the tractor at time k, Q1 represents the weight coefficient for the process points of the tractor's planned trajectory, W1 represents the weight coefficient for the endpoint of the tractor's planned trajectory, v1 represents the planned speed of the tractor, Δt represents the second calculation step size, L1 represents the distance from the center of the front axle to the center of the rear axle of the tractor, and δ max This indicates the preset front wheel steering angle threshold.

[0053] As can be seen from the above scheme, the tractor path tracking control model provided in this application can improve the stability of the entire system by introducing a planned trajectory endpoint state penalty term in the optimization objective. Furthermore, considering the tractor front wheel steering angle tracking error penalty term in the optimization objective is equivalent to introducing a feedforward control quantity in the tractor model predictive control, which can improve the stability and convergence speed of the entire system.

[0054] Secondly, embodiments of this application provide a trajectory planning and tracking control device for an articulated vehicle, the articulated vehicle including a tractor and a semi-trailer articulated with the tractor, the device comprising:

[0055] The first trajectory planning module is used to search the first planned trajectory of the tractor using a heuristic search algorithm;

[0056] The second trajectory planning module is used to determine the second trajectory point information corresponding to each first trajectory point information in the first planned trajectory, based on the first trajectory point information, the non-time reference dynamic equation about the target angle between the tractor and the semi-trailer, and the geometric relationship between the tractor and the semi-trailer, and to generate the second planned trajectory of the semi-trailer based on multiple second trajectory point information. The non-time reference dynamic equation is a discretized equation with the target angle as the state variable and the front wheel rotation angle of the tractor as the control variable, and the second trajectory point information is the trajectory point information of the semi-trailer.

[0057] The first tracking control module is used to solve the semi-trailer path tracking control model by taking the second planned trajectory as the desired trajectory of the semi-trailer, and to take the optimal solution of the semi-trailer path tracking control model as the tracking control quantity of the semi-trailer. The semi-trailer path tracking control model is a path tracking control model with the optimization objective of minimizing the weighted average tracking error of the semi-trailer lateral tracking error, the tracking error of the semi-trailer yaw angle, and the tracking error of the target angle between the tractor and the semi-trailer.

[0058] The second tracking control module is used to solve the tractor path tracking control model by taking the first planned trajectory as the desired trajectory of the tractor, and to take the optimal solution of the tractor path tracking control model as the tracking control quantity of the tractor. The tractor path tracking control model is a path tracking control model with the optimization objective of minimizing the weighted average tracking error of the tractor lateral tracking error, the tracking error of the tractor yaw angle, and the tracking error of the tractor front wheel steering angle.

[0059] In one possible implementation, the basic action units of the heuristic search algorithm include a spiral basic action unit, a circular arc basic action unit, and a straight line segment basic action unit.

[0060] The basic spiral motion unit includes a spiral line from a straight line segment to a spiral line with a first preset radius, a spiral line from a first target radius to a second target radius, a spiral line from a second target preset radius to a first target radius, and a spiral line from the first preset radius to a straight line segment. The first target radius and the second target radius are two adjacent preset radii among a plurality of preset radii set in a preset order, and the first target radius includes the first preset radius.

[0061] The basic arc motion unit includes an arc with radii of each of the plurality of preset radii, which extends toward the target direction;

[0062] The angular change is the same for each basic spiral motion unit and each basic circular arc motion unit.

[0063] The first trajectory planning module is used to perform a heuristic search on the planned trajectory of the tractor vehicle by splicing together the basic motion units of the spiral, the basic motion units of the circular arc and the basic motion units of the straight line segment, according to the principle of continuous curvature of the planned trajectory, so as to obtain the first planned trajectory.

[0064] In one possible implementation, the first trajectory planning module is used to search for the first trajectory point information of the tractor at time k+1 using the heuristic search algorithm; calculate the target angle at time k+1 based on the target angle at time k, the front wheel angle of the tractor at time k, the distance from the midpoint of the rear axle of the tractor to the articulation point, the distance from the articulation point to the midpoint of the rear axle of the semi-trailer, the distance from the midpoint of the front axle of the tractor to the midpoint of the rear axle, the first calculation step size, and the non-time reference dynamic equation; when the target angle at time k+1 is greater than or equal to a preset angle threshold, the heuristic search algorithm is used again to search for the first trajectory point information of the tractor at time k+1.

[0065] In one possible implementation, the preset included angle threshold is determined based on the distance from the midpoint of the tractor's rear axle to the hinge point, the distance from the hinge point to the midpoint of the semi-trailer's rear axle, and the minimum turning radius of the tractor.

[0066] In one possible implementation, the preset included angle threshold includes:

[0067]

[0068] Wherein, the β max The preset included angle threshold is represented by M1, which represents the distance from the midpoint of the rear axle of the tractor to the articulation point, and L2 represents the distance from the articulation point to the midpoint of the rear axle of the semi-trailer. R... min This indicates the minimum turning radius of the tractor unit.

[0069] In one possible implementation, the second trajectory planning module includes:

[0070] The included angle calculation submodule is used to calculate the target included angle at time k+1 based on the target included angle at time k, the turning angle of the front wheel of the tractor at time k, the distance from the midpoint of the rear axle of the tractor to the hinge point, the distance from the hinge point to the midpoint of the rear axle of the semi-trailer, the distance from the midpoint of the front axle of the tractor to the midpoint of the rear axle, the first calculation step size, and the non-time reference dynamic equation.

[0071] The state calculation submodule is used to calculate the planned pose of the semi-trailer at the target time based on the planned pose of the tractor at the target time in the first trajectory point information and the geometric relationship between the tractor and the semi-trailer, and to calculate other state information of the semi-trailer at the target time based on other state information of the tractor at the target time in the first trajectory point information and the geometric relationship between the tractor and the semi-trailer, wherein the other state information includes at least the planned vehicle speed.

[0072] In one possible implementation, the non-time-referenced dynamic equations include:

[0073] β k+1 =β k +hK2

[0074] in,

[0075]

[0076] Wherein, the β k+1 β k Let K1 and K2 represent the target angles at time (k+1) and time (k), respectively; let h represent the first calculation step size; let K1 and K2 represent the coefficients of the second-order Runge-Kutta integral method; and let δ... k L1 represents the turning angle of the front wheel of the tractor at time k, L2 represents the distance from the center of the front axle to the center of the rear axle of the tractor, M1 represents the distance from the articulation point to the midpoint of the rear axle of the semi-trailer, and M2 represents the distance from the midpoint of the rear axle of the tractor to the articulation point.

[0077] In one possible implementation, the device further includes:

[0078] The first calculation module is used to calculate the temporary planned speed of the semi-trailer by using the geometric relationship between the speeds of the tractor and the semi-trailer and the planned speed of the tractor included in the first planned trajectory.

[0079] The deduction module is used to deduce the predicted trajectory length of the semi-trailer based on the temporary planned speed and planning duration of the semi-trailer;

[0080] The conversion module is used to convert the predicted trajectory length of the semi-trailer into the predicted trajectory length of the tractor using the association model of the tractor and the semi-trailer.

[0081] The correction module is used to correct the planned speed of the tractor in the first planned trajectory based on the predicted trajectory length of the tractor, so as to obtain the corrected planned speed of the tractor.

[0082] In one possible implementation, the device further includes:

[0083] The adjustment module is used to adjust the planned speed of the tractor vehicle after correction using the sliding window method, so that the planned speed of the tractor vehicle changes in advance.

[0084] The smoothing module is used to smooth the planned vehicle speed after the sliding window method is processed by the constraint optimization method, and obtain the smoothed planned vehicle speed of the tractor as the final planned vehicle speed.

[0085] The second calculation module is used to calculate the smoothed planned speed of the semi-trailer as the final planned speed by utilizing the geometric relationship between the speeds of the tractor and the semi-trailer and the smoothed planned speed of the tractor.

[0086] In one possible implementation, the semi-trailer path tracking control model includes:

[0087]

[0088] Wherein, the ω f (k) represents the front wheel angular velocity of the semi-trailer to be tracked and controlled at time k, N represents time N, y2(k) and y2(k+1) represent the lateral position coordinates of the semi-trailer to be tracked and controlled at time k and time k+1, respectively, x2(k) and x2(k+1) represent the longitudinal position coordinates of the semi-trailer to be tracked and controlled at time k and time k+1, respectively, θ2(k) and θ2(k+1) represent the yaw angles of the semi-trailer to be tracked and controlled at time k and time k+1, respectively, and β k The y represents the included angle of the target to be tracked and controlled at time k. 2,ref (k) represents the expected lateral position coordinate of the semi-trailer at time k, where θ 2,ref (k) represents the expected yaw angle of the semi-trailer at time k, and β ref (k) represents the expected target angle of the semi-trailer at time k, Q2 represents the weight coefficient for the process points of the semi-trailer's planned trajectory, W2 represents the weight coefficient for the endpoint of the semi-trailer's planned trajectory, v2 represents the planned speed of the semi-trailer, Δt represents the second calculation step size, L2 represents the distance from the articulation point to the midpoint of the semi-trailer's rear axle, and β max This indicates the preset included angle threshold.

[0089] In one possible implementation, the tractor path tracking control model includes:

[0090]

[0091] Wherein, δ(k) represents the front wheel steering angle of the tractor to be tracked and controlled at time k, N represents time N, y1(k) and y1(k+1) represent the lateral position coordinates of the tractor to be tracked and controlled at time k and time k+1, respectively, x1(k) and x1(k+1) represent the longitudinal position coordinates of the tractor to be tracked and controlled at time k and time k+1, respectively, θ1(k) and θ1(k+1) represent the yaw angle of the tractor to be tracked and controlled at time k and time k+1, respectively. 1,ref (k) represents the desired lateral position coordinate of the tractor at time k, where θ 1,ref (k) represents the expected yaw angle of the tractor at time k, and the δ ref (k) represents the desired target angle of the tractor at time k, Q1 represents the weight coefficient for the process points of the tractor's planned trajectory, W1 represents the weight coefficient for the endpoint of the tractor's planned trajectory, v1 represents the planned speed of the tractor, Δt represents the second calculation step size, L1 represents the distance from the center of the front axle to the center of the rear axle of the tractor, and δ max This indicates the preset front wheel steering angle threshold.

[0092] As can be seen from the above scheme, the embodiments of this application can first plan the curvature-continuous tractor trajectory and semi-trailer trajectory based on the heuristic search algorithm, the non-time reference dynamic equation of the target angle between the tractor and the semi-trailer, and the geometric relationship between the tractor and the semi-trailer. Then, using a path tracking control model with the optimization objective of minimizing the weighted average of the semi-trailer's lateral tracking error, semi-trailer's yaw angle tracking error, and the target angle tracking error between the tractor and the semi-trailer, the semi-trailer is tracked and controlled. Using a path tracking control model with the optimization objective of minimizing the weighted average of the tractor's lateral tracking error, tractor's yaw angle tracking error, and tractor's front wheel steering angle tracking error, the tractor is tracked and controlled. Thus, the automatic driving function of the articulated vehicle can be realized while meeting the requirements of safety and comfort.

[0093] Thirdly, embodiments of this application provide a computer-readable storage medium having a computer program stored thereon that, when executed by a processor, implements the method as described in any possible implementation of the first aspect.

[0094] Fourthly, embodiments of this application provide an electronic device, which includes:

[0095] One or more processors;

[0096] The processor is coupled to a storage device for storing one or more programs;

[0097] When one or more programs are executed by one or more processors, the electronic device performs the method as described in any possible implementation of the first aspect.

[0098] Fifthly, embodiments of this application provide an articulated vehicle that includes the means as described in any possible implementation of the second aspect, or includes electronic equipment as described in the fourth aspect.

[0099] In a sixth aspect, embodiments of this application provide a computer program product containing instructions that, when executed on a computer or processor, cause the computer or processor to perform the method described in any possible implementation of the first aspect. Attached Figure Description

[0100] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the accompanying drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are merely some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without any creative effort.

[0101] Figure 1 A flowchart illustrating a trajectory planning and tracking control method for an articulated vehicle provided in an embodiment of this application;

[0102] Figure 2 A schematic diagram of a basic action unit for heuristic search provided in an embodiment of this application;

[0103] Figure 3 An example diagram illustrating the adjustment of the planned speed of a tractor based on the sliding window method, provided in an embodiment of this application;

[0104] Figure 4 A schematic diagram of the geometric relationship of a kinematic model of an articulated vehicle provided in an embodiment of this application;

[0105] Figure 5 A block diagram illustrating the composition of a trajectory planning and tracking control device for an articulated vehicle, provided in an embodiment of this application.

[0106] Figure 6 This is a schematic diagram of the structure of an electronic device or computer device provided in an embodiment of this application. Detailed Implementation

[0107] The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of this application, and not all of them. All other embodiments obtained by those skilled in the art based on the embodiments of this application without creative effort are within the scope of protection of this application.

[0108] It should be noted that, unless otherwise specified, the embodiments and features described in this application can be combined with each other. The terms "comprising" and "having," and any variations thereof, in the embodiments and drawings of this application are intended to cover non-exclusive inclusion. For example, a process, method, system, product, or device that includes a series of steps or units is not limited to the listed steps or units, but may optionally include steps or units not listed, or may optionally include other steps or units inherent to these processes, methods, products, or devices.

[0109] Figure 1 This is a flowchart illustrating a trajectory planning and tracking control method for an articulated vehicle. This method can be applied to various road scenarios, such as parking and S-curve scenarios. The trailer handling assistance system and trailer caravan parking system designed using this method are both within the scope of protection claimed in this application. This method can be applied to electronic devices or computer equipment, specifically to an articulated vehicle or a server interacting with the articulated vehicle. The articulated vehicle includes a tractor and a semi-trailer articulated with the tractor. The method may include the following steps:

[0110] S110: Use a heuristic search algorithm to search for the first planned trajectory of the tractor.

[0111] The first planned trajectory includes multiple first trajectory point information. Each first trajectory point information refers to the status information of each planned trajectory point of the tractor, including planned position, planned speed, yaw angle, front wheel turning angle, etc.

[0112] In related technologies, when using heuristic search algorithms to search for non-articulated, single-carriage ordinary vehicles, the search is often carried out by splicing arcs and straight line segments. However, in articulated vehicles, in order to improve the continuity of the curvature of the planned trajectory, the embodiments of this application also introduce a spiral.

[0113] Specifically, the basic action units of the heuristic search algorithm include spiral basic action units, circular arc basic action units, and straight line segment basic action units. The spiral basic action unit includes a spiral line from a straight line segment to a spiral line with a radius of a first preset radius, a spiral line from a first target radius to a second target radius, a spiral line from a second target preset radius to a first target radius, and a spiral line from a first preset radius to a straight line segment. The first target radius and the second target radius are two adjacent preset radii among a plurality of preset radii set in a preset order, and the first target radius includes the first preset radius. The circular arc basic action unit includes a circular arc with radii of each of the plurality of preset radii that extends toward the target direction.

[0114] In this design, the angular change of each basic helical motion unit and circular arc motion unit is the same, which facilitates the use of a three-dimensional array to store and search the occupied grid. Target directions include left-front, left-rear, right-front, and right-rear directions.

[0115] After setting up the basic motion units, the process of using a heuristic search algorithm to search for the first planned trajectory of the tractor includes: according to the principle of continuous curvature of the planned trajectory, the planned trajectory of the tractor is searched heuristically by splicing together the basic motion units of the spiral, the basic motion units of the circular arc and the basic motion units of the straight line segment to obtain the first planned trajectory.

[0116] The three types of basic motion units mentioned above—spiral, arc, and straight line segments—can be further combined into basic motion units with larger step lengths. During the heuristic search process, the step length is set according to the distance between the vehicle and the obstacle to balance search efficiency and search precision.

[0117] like Figure 2 As shown, taking preset radii including R1, R2, and R3 as an example, the above basic motion units are explained. The basic motion unit for a spiral includes: a spiral from a straight line segment to R1, a spiral from R1 to R2, a spiral from R2 to R3, a spiral from R3 to R2, a spiral from R2 to R1, and a spiral from R1 to a straight line segment. The basic motion unit for an arc includes: arcs extending to the left and forward with radii R1, R2, and R3; arcs extending to the right and forward with radii R1, R2, and R3; arcs extending to the left and backward with radii R1, R2, and R3; and arcs extending to the right and backward with radii R1, R2, and R3. The angular change of each spiral and arc basic motion unit is Δθ, which facilitates the use of a three-dimensional array for storing and searching occupied grid cells. Meanwhile, the principle of splicing basic motion units is to ensure the continuity of path curvature. For example, the subsequent basic motion units of the spiral from R1 to R2 include: an arc with radius R2, a spiral from R2 to R1, and a spiral from R2 to R3.

[0118] The basic helical motion unit needs to control the change in the front wheel angle of the tractor during the movement of the tractor-semi-trailer, ensuring a continuous change in the curvature of the planned trajectory. To ensure the constraint on the rate of change of the tractor's front wheel angle, the maximum allowable speed of the basic helical motion unit needs to be limited. The smaller the maximum allowable speed, the greater the difference in curvature between the two endpoints of the fixed-length basic helical motion unit. Considering the maximum allowable speed of the basic helical motion unit, the cost of heuristic search needs to take into account the time from the planning start point to the planning end point, thus suppressing the basic helical motion unit. Simultaneously, the cost of heuristic search also includes: the number of gear shifts, the tractor's front wheel angle, the rate of change of the tractor's front wheel angle, the path length, the distance to obstacles, and the angle between the tractor and semi-trailer.

[0119] Considering the alternating splicing of basic motion units of circular arcs and spirals, the maximum allowable speed corresponding to the planned trajectory may fluctuate frequently. To suppress these fluctuations, a sliding window method is used to adjust the planned speed of the tractor obtained by the heuristic search algorithm, causing the tractor's planned speed to change gears earlier, such as by slowing down or accelerating earlier. Then, a constraint optimization method is used to smooth the planned speed processed by the sliding window method, obtaining a smoothed planned speed for the tractor. Using the geometric relationship between the speeds of the tractor and the semi-trailer, and the smoothed planned speed of the tractor, the smoothed planned speed of the semi-trailer is calculated. Here, the planned speed is the maximum allowable speed. Figure 3 As shown, after adjusting the planned speed of the tractor obtained by the heuristic search algorithm using the sliding window method, the tractor will slow down in advance. The advance distance is the aiming distance. This aiming distance can make the smoothing effect obtained by subsequent constraint optimization more stable, or in other words, the curvature change more stable.

[0120] The geometric relationship between the speeds of the tractor and the semi-trailer includes:

[0121]

[0122] Where v1 and v2 represent the planned speeds of the tractor and semi-trailer, respectively, β represents the target angle between the tractor and semi-trailer, M1 represents the distance from the midpoint of the tractor's rear axle to the articulation point, and θ1 represents the yaw angle of the tractor relative to the geodetic coordinate system.

[0123] In one possible implementation, to ensure that both the tractor and the semi-trailer maintain steady circular motion when turning and avoid folding instability, the process of searching the first planned trajectory of the tractor using a heuristic search algorithm may further include: searching for the first trajectory point information of the tractor at time k+1 using a heuristic search algorithm; calculating the target angle at time k+1 based on the target angle at time k, the turning angle of the tractor's front wheel at time k, the distance from the midpoint of the tractor's rear axle to the hinge point, the distance from the hinge point to the midpoint of the semi-trailer's rear axle, the distance from the midpoint of the tractor's front axle to the midpoint of the rear axle, the first calculation step size, and the non-time reference dynamic equation; and when the target angle at time k+1 is greater than or equal to a preset angle threshold, re-searching for the first trajectory point information of the tractor at time k+1 using the heuristic search algorithm.

[0124] The non-time-referenced dynamic equations include:

[0125] β k+1 =β k +hK2

[0126] in,

[0127]

[0128] Where, β k+1 β k Let K1 and K2 represent the target angles at time (k+1) and time (k), respectively; h represents the first calculation step size; K1 and K2 represent the coefficients of the second-order Runge-Kutta integral method, respectively; and δ k L1 represents the turning angle of the front wheel of the tractor at time k, L2 represents the distance from the center of the front axle to the center of the rear axle of the tractor, L3 represents the distance from the articulation point to the midpoint of the rear axle of the semi-trailer, and M4 represents the distance from the midpoint of the rear axle of the tractor to the articulation point.

[0129] The preset included angle threshold is determined based on the distance from the midpoint of the tractor's rear axle to the articulation point, the distance from the articulation point to the midpoint of the semi-trailer's rear axle, and the tractor's minimum turning radius.

[0130] Preset included angle thresholds include:

[0131]

[0132] Where, β max This indicates the preset included angle threshold, M1 represents the distance from the midpoint of the tractor's rear axle to the articulation point, L2 represents the distance from the articulation point to the midpoint of the semi-trailer's rear axle, and R... min This indicates the minimum turning radius of the tractor unit.

[0133] The reasoning process for the non-time-referenced dynamic equations is explained in detail below:

[0134] like Figure 4As shown, establish a geodetic coordinate system OXY and a tractor coordinate system Axy. Define the velocity v1 at the midpoint A of the tractor's rear axle as positive for forward gear and negative for reverse gear. The rotation angle δ of the tractor's front wheels relative to coordinate system Axy is positive for counter-clockwise rotation and negative for clockwise rotation. The yaw angle of the tractor relative to the geodetic coordinate system OXY is θ1, the yaw angle of the semi-trailer relative to the geodetic coordinate system OXY is θ2, the target angle between the tractor and the semi-trailer is β, and the velocity vector at the hinge point P is v. p By applying orthogonal decomposition of velocities, the yaw rates of the tractor and semi-trailer can be obtained as follows:

[0135]

[0136] In the formula, M1 is the distance from the midpoint A of the rear axle of the tractor to the hinge point P; L2 is the distance from the hinge point P to the midpoint B of the rear axle of the semi-trailer; O1 and O2 are the centers of the circles corresponding to the turning radius of the tractor and the semi-trailer, respectively.

[0137] Combining equations (1) and (2), the rate of change of the target angle between the tractor and the semi-trailer can be obtained.

[0138]

[0139] like Figure 4 As shown, the velocity relationship can be obtained from trigonometric function formulas.

[0140]

[0141] Combining equations (2) to (4), the rate of change of the target angle between the tractor and the semi-trailer can be obtained.

[0142]

[0143] Furthermore, the yaw rate of the tractor can be expressed as:

[0144]

[0145] In the formula, L1 is the distance from the center of the front axle to the center of the rear axle of the tractor.

[0146] Substituting equation (6) into equation (5), we get

[0147]

[0148] Let 's' represent the path length, i.e., the trajectory length, and utilize... The equation (7) is transformed into a non-time-referenced dynamic equation by v1 = ds / dt.

[0149]

[0150] Using the target angle β as the state variable and the front wheel rotation angle δ of the tractor as the control variable, the above discrete non-time reference dynamic equation can be obtained by discretizing equation (8) using the second-order Runge-Kutta integral method.

[0151] S120: For each first trajectory point in the first planned trajectory, based on the first trajectory point information, the non-time reference dynamic equation about the target angle between the tractor and the semi-trailer, and the geometric relationship between the tractor and the semi-trailer, determine the second trajectory point information corresponding to the first trajectory point information, and generate the second planned trajectory of the semi-trailer based on multiple second trajectory point information.

[0152] Among them, the non-time reference dynamic equation is a discretized equation with the target included angle as the state variable and the front wheel rotation angle of the tractor as the control variable. The second trajectory point information is the trajectory point information of the semi-trailer. Each second trajectory point information refers to the state information of each planned trajectory point of the semi-trailer, including the planned position, planned speed, yaw angle, front wheel rotation angle, etc.

[0153] The specific implementation process of step S120 may include: calculating the target angle at time k+1 based on the target angle at time k, the tractor's front wheel rotation angle at time k, the distance from the midpoint of the tractor's rear axle to the articulation point, the distance from the articulation point to the midpoint of the semi-trailer's rear axle, the distance from the midpoint of the tractor's front axle to the midpoint of the rear axle, the first calculation step size, and the non-time reference dynamic equation. That is, by calculating the target angle at time k, the tractor's front wheel rotation angle at time k, the distance from the midpoint of the tractor's rear axle to the articulation point, the distance from the articulation point to the midpoint of the semi-trailer's rear axle, and the distance from the tractor's front axle to the articulation point, the distance from the midpoint of the tractor's front axle to the midpoint of the rear axle, and the distance from the tractor's front axle to the articulation point, the distance from the articulation point to the midpoint of the semi-trailer's rear axle, and the distance from the tractor's front axle to the articulation point, the distance from the midpoint of the tractor's front axle to the articulation point, and the distance from the midpoint of the tractor's front axle to the articulation point, the distance from the midpoint of the tractor's front axle to the articulation point, and the distance from the midpoint of the articulation point to the midpoint of the semi-trailer's rear axle, the distance from the midpoint of the tractor's front axle to the articulation point, and the distance from the midpoint of the tractor's front axle to the articulation point, the distance from the midpoint of the tractor's front axle to the articulation point, the distance from The distance from the midpoint to the midpoint of the rear axle and the first calculation step size are substituted into the non-time reference dynamic equation to calculate the target angle at time k+1. For the same target time, the planned pose of the semi-trailer at the target time is calculated based on the planned pose of the tractor at the target time and the geometric relationship between the tractor and the semi-trailer in the first trajectory point information. Other state information of the semi-trailer at the target time is calculated based on other state information of the tractor at the target time and the geometric relationship between the tractor and the semi-trailer in the first trajectory point information. Other state information includes at least the planned vehicle speed.

[0154] The geometric relationship between the tractor and the semi-trailer used in calculating the planned pose of the semi-trailer at the target time includes:

[0155]

[0156] Where θ2(k) represents the yaw angle of the semi-trailer at time k, θ1(k) represents the yaw angle of the tractor at time k, x1(k) represents the longitudinal position coordinate of the tractor at time k, x2(k) represents the longitudinal position coordinate of the semi-trailer at time k, y1(k) represents the lateral position coordinate of the tractor at time k, x1(k) represents the lateral position coordinate of the semi-trailer at time k, M1 is the distance from the midpoint of the rear axle of the tractor to the articulation point, and L2 is the distance from the articulation point to the midpoint of the rear axle of the semi-trailer.

[0157] S130: Solve the semi-trailer path tracking control model by taking the second planned trajectory as the desired trajectory of the semi-trailer, and take the optimal solution of the semi-trailer path tracking control model as the tracking control quantity of the semi-trailer.

[0158] Among them, the semi-trailer path tracking control model is a path tracking control model with the optimization objective of minimizing the weighted average of the semi-trailer lateral tracking error, the semi-trailer yaw angle tracking error, and the target angle tracking error between the tractor and the semi-trailer.

[0159] First, the kinematic models of the tractor unit, the semi-trailer unit, and the tractor-semi-trailer correlation model are established, including:

[0160]

[0161] In the formula, v2 is the speed of the midpoint of the rear axle of the semi-trailer.

[0162] Based on equation (9), the cascaded predictive control model is obtained as follows:

[0163]

[0164] In the formula, Δt is the second calculation step size.

[0165] Since the dynamic relationship between the rate of change of the tractor's front wheel angle and the front wheel angle does not include the speed v1 of the tractor's rear axle midpoint A, and the dynamic relationship between the rate of change of the target angle between the tractor and the semi-trailer and the target angle does not include the speed v2 of the semi-trailer's rear axle midpoint B, the non-time reference cascade prediction model based on path length is difficult to extend to include the dynamic relationship between the rate of change of the tractor's front wheel angle and the front wheel angle, and the dynamic relationship between the rate of change of the target angle and the target angle. Therefore, compared with the non-time reference cascade prediction model based on path length, the cascade prediction model described by Equation (10) uses the speed v1 of the tractor's rear axle midpoint A and the speed v2 of the semi-trailer's rear axle midpoint B as adjustable parameters affecting the predicted path length of the two, and has a greater advantage from the perspective of extending to include the dynamic relationship between the rate of change of the tractor's front wheel angle and the front wheel angle, and the dynamic relationship between the rate of change of the target angle and the target angle.

[0166] With the optimization objective of minimizing the weighted average of the semi-trailer's lateral tracking error, yaw angle tracking error, and target angle tracking error, the model-predicted semi-trailer path tracking control subproblem is transformed into the following nonlinear constrained optimization subproblem, thus obtaining the semi-trailer path tracking control model, including:

[0167]

[0168] Where, ω f (k) represents the front wheel angular velocity of the semi-trailer to be tracked and controlled at time k, N represents time N, y2(k) and y2(k+1) represent the lateral position coordinates of the semi-trailer to be tracked and controlled at time k and time k+1, respectively, x2(k) and x2(k+1) represent the longitudinal position coordinates of the semi-trailer to be tracked and controlled at time k and time k+1, respectively, θ2(k) and θ2(k+1) represent the yaw angles of the semi-trailer to be tracked and controlled at time k and time k+1, respectively, β k y represents the included angle of the target to be tracked and controlled at time k. 2,ref (k) represents the expected lateral position coordinate of the semi-trailer at time k, θ 2,ref (k) represents the expected yaw angle of the semi-trailer at time k, β ref (k) represents the expected target angle of the semi-trailer at time k, Q2 represents the weight coefficient for the process points of the semi-trailer's planned trajectory, W2 represents the weight coefficient for the endpoint of the semi-trailer's planned trajectory, v2 represents the planned speed of the semi-trailer, Δt represents the second calculation step size, L2 represents the distance from the articulation point to the midpoint of the semi-trailer's rear axle, and β max This indicates the preset included angle threshold.

[0169] The above-mentioned semi-trailer path tracking control model introduces a penalty term for the final state of the planned trajectory into the optimization objective. This can improve the stability of the entire system. Furthermore, considering the target angle tracking error between the tractor and the semi-trailer in the optimization objective is equivalent to introducing a feedforward control variable in the semi-trailer model predictive control, which can improve the stability and convergence speed of the entire system.

[0170] S140: Solve the tractor path tracking control model by taking the first planned trajectory as the desired trajectory of the tractor, and use the optimal solution of the tractor path tracking control model as the tracking control quantity of the tractor.

[0171] Among them, the tractor path tracking control model is a path tracking control model with the optimization objective of minimizing the weighted average of the tractor's lateral tracking error, tractor's yaw angle tracking error, and tractor's front wheel steering angle tracking error.

[0172] The optimal solution is obtained by solving the nonlinear constrained optimization subproblem described by the semi-trailer path tracking control model. Furthermore, using the tractor-trailer association model in equation (10), the reference quantity for tractor path tracking control is obtained as follows:

[0173]

[0174] The tractor path tracking control reference quantity described by equation (11) is resampled, and the optimization objective is to minimize the weighted average of the tractor lateral tracking error, yaw angle tracking error, and tractor front wheel steering angle tracking error. The tractor path tracking control subproblem based on model prediction is transformed into the following nonlinear constrained optimization subproblem, thus obtaining the tractor path tracking control model, including:

[0175]

[0176] Where δ(k) represents the front wheel steering angle of the tractor to be tracked and controlled at time k, N represents time N, y1(k) and y1(k+1) represent the lateral position coordinates of the tractor to be tracked and controlled at time k and time k+1, respectively, x1(k) and x1(k+1) represent the longitudinal position coordinates of the tractor to be tracked and controlled at time k and time k+1, respectively, θ1(k) and θ1(k+1) represent the yaw angles of the tractor to be tracked and controlled at time k and time k+1, respectively. 1,ref (k) represents the desired lateral position coordinate of the tractor at time k, θ 1,ref (k) represents the expected yaw angle of the tractor at time k, δ ref (k) represents the desired target angle of the tractor at time k, Q1 represents the weight coefficient for the process points of the tractor's planned trajectory, W1 represents the weight coefficient for the endpoint of the tractor's planned trajectory, v1 represents the planned speed of the tractor, Δt represents the second calculation step size, L1 represents the distance from the center of the front axle to the center of the rear axle of the tractor, and δ max This indicates the preset front wheel steering angle threshold.

[0177] The above-mentioned tractor path tracking control model introduces a penalty term for the final state of the planned trajectory into the optimization objective. It can improve the stability of the entire system; and considering the penalty term for the tracking error of the front wheel of the tractor in the optimization objective is equivalent to introducing a feedforward control quantity in the model predictive control of the tractor, which can improve the stability and convergence speed of the entire system.

[0178] Solving the nonlinear constrained optimization subproblem described by the tractor path tracking control model yields the final control value for the tractor's front wheel steering angle. Furthermore, if the tractor-semi-trailer is parking in a confined space, obstacle boundary constraints can be added to the nonlinear constrained optimization subproblem described by the tractor path tracking control model to improve the safety of the tractor-semi-trailer parking process.

[0179] The trajectory planning and tracking control method for articulated vehicles provided in this application first plans a curvature-continuous tractor trajectory and a semi-trailer trajectory based on a heuristic search algorithm, a non-time-reference dynamic equation regarding the target angle between the tractor and the semi-trailer, and the geometric relationship between the tractor and the semi-trailer. Then, a path tracking control model with the optimization objective of minimizing the weighted average of the semi-trailer's lateral tracking error, yaw angle tracking error, and target angle tracking error between the tractor and the semi-trailer is used to track and control the semi-trailer. Similarly, a path tracking control model with the optimization objective of minimizing the weighted average of the tractor's lateral tracking error, yaw angle tracking error, and front wheel steering angle tracking error is used to track and control the tractor. Thus, while meeting safety and comfort requirements, the autonomous driving function of the articulated vehicle can be realized.

[0180] In one possible implementation, to further improve the reliability of the planned vehicle speed, after obtaining the first planned trajectory of the tractor using a heuristic search algorithm, the planned vehicle speed can be adjusted as follows:

[0181] Using the geometric relationship between the speeds of the tractor and the semi-trailer, and the planned speed of the tractor included in the first planned trajectory, the temporary planned speed of the semi-trailer is calculated. Based on the temporary planned speed and planning duration of the semi-trailer, the predicted trajectory length of the semi-trailer is deduced. Using the association model between the tractor and the semi-trailer, the predicted trajectory length of the semi-trailer is converted into the predicted trajectory length of the tractor. Based on the predicted trajectory length of the tractor, the planned speed of the tractor in the first planned trajectory is corrected to obtain the corrected planned speed of the tractor.

[0182] In extrapolating the predicted trajectory length of the semi-trailer, the planned speed of the tractor included in the first planned trajectory can be substituted into the geometric relationship between the speeds of the tractor and the semi-trailer to calculate the temporary planned speed of the semi-trailer corresponding to each planned speed of the tractor. Then, the product of the planned duration corresponding to each planned speed and the temporary planned speed of the semi-trailer can be calculated. The predicted trajectory length of the semi-trailer can be extrapolated by summing up the various products.

[0183] Furthermore, to suppress frequent fluctuations in the maximum allowable speed corresponding to the planned trajectory, making speed changes smoother and vehicle operation more stable and safer, after correcting the planned speed of the tractor, the sliding window method can be used to adjust the corrected planned speed of the tractor, so that the planned speed of the tractor changes in advance; the constrained optimization method is used to smooth the planned speed after the sliding window method, and the smoothed planned speed of the tractor is obtained as the final planned speed; using the geometric relationship between the speeds of the tractor and the semi-trailer, and the smoothed planned speed of the tractor, the smoothed planned speed of the semi-trailer is calculated as the final planned speed.

[0184] The smoothing scheme is based on the same principle as the smoothing scheme mentioned in the detailed explanation of step S110. However, in the detailed explanation of step S110, the first planned trajectory is smoothed directly, while in the embodiment of this application, the planned vehicle speed in the first planned trajectory is corrected first, and then the corrected planned vehicle speed is smoothed. Therefore, the latter has higher reliability of the planned vehicle speed.

[0185] Corresponding to the above method embodiments, another embodiment of this application provides a trajectory planning and tracking control device for an articulated vehicle, wherein the articulated vehicle includes a tractor and a semi-trailer articulated with the tractor, such as... Figure 5 As shown, the device includes:

[0186] The first trajectory planning module 210 is used to search the first planned trajectory of the tractor using a heuristic search algorithm;

[0187] The second trajectory planning module 220 is used to determine the second trajectory point information corresponding to each first trajectory point information in the first planned trajectory based on the first trajectory point information, the non-time reference dynamic equation about the target angle between the tractor and the semi-trailer, and the geometric relationship between the tractor and the semi-trailer, and to generate the second planned trajectory of the semi-trailer based on multiple second trajectory point information. The non-time reference dynamic equation is a discretized equation with the target angle as the state variable and the front wheel rotation angle of the tractor as the control variable, and the second trajectory point information is the trajectory point information of the semi-trailer.

[0188] The first tracking control module 230 is used to solve the semi-trailer path tracking control model by taking the second planned trajectory as the expected trajectory of the semi-trailer, and to take the optimal solution of the semi-trailer path tracking control model as the tracking control quantity of the semi-trailer. The semi-trailer path tracking control model is a path tracking control model with the optimization objective of minimizing the weighted average tracking error of the semi-trailer lateral tracking error, the tracking error of the semi-trailer yaw angle, and the tracking error of the target angle between the tractor and the semi-trailer.

[0189] The second tracking control module 240 is used to solve the tractor path tracking control model by taking the first planned trajectory as the expected trajectory of the tractor, and to take the optimal solution of the tractor path tracking control model as the tracking control quantity of the tractor. The tractor path tracking control model is a path tracking control model with the optimization objective of minimizing the weighted average tracking error of the tractor lateral tracking error, the tracking error of the tractor yaw angle, and the tracking error of the tractor front wheel steering angle.

[0190] In one possible implementation, the basic action units of the heuristic search algorithm include a spiral basic action unit, a circular arc basic action unit, and a straight line segment basic action unit.

[0191] The basic spiral motion unit includes a spiral line from a straight line segment to a spiral line with a first preset radius, a spiral line from a first target radius to a second target radius, a spiral line from a second target preset radius to a first target radius, and a spiral line from the first preset radius to a straight line segment. The first target radius and the second target radius are two adjacent preset radii among a plurality of preset radii set in a preset order, and the first target radius includes the first preset radius.

[0192] The basic arc motion unit includes an arc with radii of each of the plurality of preset radii, which extends toward the target direction;

[0193] The angular change is the same for each basic spiral motion unit and each basic circular arc motion unit.

[0194] The first trajectory planning module 210 is used to perform a heuristic search on the planned trajectory of the tractor vehicle by splicing together the basic motion units of the spiral, the basic motion units of the circular arc and the basic motion units of the straight line segment, according to the principle of continuous curvature of the planned trajectory, so as to obtain the first planned trajectory.

[0195] In one possible implementation, the first trajectory planning module 210 is used to search for the first trajectory point information of the tractor at time k+1 using the heuristic search algorithm; calculate the target angle at time k+1 based on the target angle at time k, the front wheel angle of the tractor at time k, the distance from the midpoint of the rear axle of the tractor to the articulation point, the distance from the articulation point to the midpoint of the rear axle of the semi-trailer, the distance from the midpoint of the front axle of the tractor to the midpoint of the rear axle, the first calculation step size, and the non-time reference dynamic equation; when the target angle at time k+1 is greater than or equal to a preset angle threshold, the heuristic search algorithm is used again to search for the first trajectory point information of the tractor at time k+1.

[0196] In one possible implementation, the preset included angle threshold is determined based on the distance from the midpoint of the tractor's rear axle to the hinge point, the distance from the hinge point to the midpoint of the semi-trailer's rear axle, and the minimum turning radius of the tractor.

[0197] In one possible implementation, the preset included angle threshold includes:

[0198]

[0199] Wherein, the β maxThe preset included angle threshold is represented by M1, which represents the distance from the midpoint of the rear axle of the tractor to the articulation point, and L2 represents the distance from the articulation point to the midpoint of the rear axle of the semi-trailer. R... min This indicates the minimum turning radius of the tractor unit.

[0200] In one possible implementation, the second trajectory planning module 220 includes:

[0201] The included angle calculation submodule is used to calculate the target included angle at time k+1 based on the target included angle at time k, the turning angle of the front wheel of the tractor at time k, the distance from the midpoint of the rear axle of the tractor to the hinge point, the distance from the hinge point to the midpoint of the rear axle of the semi-trailer, the distance from the midpoint of the front axle of the tractor to the midpoint of the rear axle, the first calculation step size, and the non-time reference dynamic equation.

[0202] The state calculation submodule is used to calculate the planned pose of the semi-trailer at the target time based on the planned pose of the tractor at the target time in the first trajectory point information and the geometric relationship between the tractor and the semi-trailer, and to calculate other state information of the semi-trailer at the target time based on other state information of the tractor at the target time in the first trajectory point information and the geometric relationship between the tractor and the semi-trailer, wherein the other state information includes at least the planned vehicle speed.

[0203] In one possible implementation, the non-time-referenced dynamic equations include:

[0204] β k+1 =β k +hK2

[0205] in,

[0206]

[0207] Wherein, the β k+1 β k Let K1 and K2 represent the target angles at time (k+1) and time (k), respectively; let h represent the first calculation step size; let K1 and K2 represent the coefficients of the second-order Runge-Kutta integral method; and let δ... k L1 represents the turning angle of the front wheel of the tractor at time k, L2 represents the distance from the center of the front axle to the center of the rear axle of the tractor, M1 represents the distance from the articulation point to the midpoint of the rear axle of the semi-trailer, and M2 represents the distance from the midpoint of the rear axle of the tractor to the articulation point.

[0208] In one possible implementation, the device further includes:

[0209] The first calculation module is used to calculate the temporary planned speed of the semi-trailer by using the geometric relationship between the speeds of the tractor and the semi-trailer and the planned speed of the tractor included in the first planned trajectory.

[0210] The deduction module is used to deduce the predicted trajectory length of the semi-trailer based on the temporary planned speed and planning duration of the semi-trailer;

[0211] The conversion module is used to convert the predicted trajectory length of the semi-trailer into the predicted trajectory length of the tractor using the association model of the tractor and the semi-trailer.

[0212] The correction module is used to correct the planned speed of the tractor in the first planned trajectory based on the predicted trajectory length of the tractor, so as to obtain the corrected planned speed of the tractor.

[0213] In one possible implementation, the device further includes:

[0214] The adjustment module is used to adjust the planned speed of the tractor vehicle after correction using the sliding window method, so that the planned speed of the tractor vehicle changes in advance.

[0215] The smoothing module is used to smooth the planned vehicle speed after the sliding window method is processed by the constraint optimization method, and obtain the smoothed planned vehicle speed of the tractor as the final planned vehicle speed.

[0216] The second calculation module is used to calculate the smoothed planned speed of the semi-trailer as the final planned speed by utilizing the geometric relationship between the speeds of the tractor and the semi-trailer and the smoothed planned speed of the tractor.

[0217] In one possible implementation, the semi-trailer path tracking control model includes:

[0218]

[0219] Wherein, the ω f (k) represents the front wheel angular velocity of the semi-trailer to be tracked and controlled at time k, N represents time N, y2(k) and y2(k+1) represent the lateral position coordinates of the semi-trailer to be tracked and controlled at time k and time k+1, respectively, x2(k) and x2(k+1) represent the longitudinal position coordinates of the semi-trailer to be tracked and controlled at time k and time k+1, respectively, θ2(k) and θ2(k+1) represent the yaw angles of the semi-trailer to be tracked and controlled at time k and time k+1, respectively, and β k The y represents the included angle of the target to be tracked and controlled at time k. 2,ref(k) represents the expected lateral position coordinate of the semi-trailer at time k, where θ 2,ref (k) represents the expected yaw angle of the semi-trailer at time k, and β ref (k) represents the expected target angle of the semi-trailer at time k, Q2 represents the weight coefficient for the process points of the semi-trailer's planned trajectory, W2 represents the weight coefficient for the endpoint of the semi-trailer's planned trajectory, v2 represents the planned speed of the semi-trailer, Δt represents the second calculation step size, L2 represents the distance from the articulation point to the midpoint of the semi-trailer's rear axle, and β max This indicates the preset included angle threshold.

[0220] In one possible implementation, the tractor path tracking control model includes:

[0221]

[0222] Wherein, δ(k) represents the front wheel steering angle of the tractor to be tracked and controlled at time k, N represents time N, y1(k) and y1(k+1) represent the lateral position coordinates of the tractor to be tracked and controlled at time k and time k+1, respectively, x1(k) and x1(k+1) represent the longitudinal position coordinates of the tractor to be tracked and controlled at time k and time k+1, respectively, θ1(k) and θ1(k+1) represent the yaw angle of the tractor to be tracked and controlled at time k and time k+1, respectively. 1,ref (k) represents the desired lateral position coordinate of the tractor at time k, where θ 1,ref (f) represents the expected yaw angle of the tractor at time k, where δ ref (f) represents the expected target angle of the tractor at time k, Q1 represents the weight coefficient for the process points of the tractor's planned trajectory, W1 represents the weight coefficient for the endpoint of the tractor's planned trajectory, v1 represents the planned speed of the tractor, Δt represents the second calculation step size, L1 represents the distance from the center of the front axle to the center of the rear axle of the tractor, and δ max This indicates the preset front wheel steering angle threshold.

[0223] The trajectory planning and tracking control device for articulated vehicles provided in this application embodiment can first plan a curvature-continuous tractor trajectory and a semi-trailer trajectory based on a heuristic search algorithm, a non-time-reference dynamic equation regarding the target angle between the tractor and the semi-trailer, and the geometric relationship between the tractor and the semi-trailer. Then, using a path tracking control model with the optimization objective of minimizing the weighted average of the semi-trailer's lateral tracking error, semi-trailer's yaw angle tracking error, and the target angle tracking error between the tractor and the semi-trailer, the semi-trailer is tracked and controlled. Using a path tracking control model with the optimization objective of minimizing the weighted average of the tractor's lateral tracking error, tractor's yaw angle tracking error, and tractor's front wheel steering angle tracking error, the tractor is tracked and controlled. Thus, while meeting requirements such as safety and comfort, the autonomous driving function of the articulated vehicle can be realized.

[0224] Based on the above method embodiments, another embodiment of this application provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the method as described in any of the above embodiments.

[0225] Based on the above method embodiments, another embodiment of this application provides an electronic device, such as... Figure 6 As shown, the electronic device includes:

[0226] One or more processors 310;

[0227] The processor 310 is coupled to a storage device 320, the storage device 320 being used to store one or more programs;

[0228] When the one or more programs are executed by the one or more processors 310, the electronic device or computer device performs the method as described in any of the above embodiments.

[0229] Based on the above method embodiments, another embodiment of this application provides an articulated vehicle that includes the device as described in any of the above embodiments, or includes the electronic equipment as described above.

[0230] Based on the above embodiments, another embodiment of this application provides a computer program product, which includes instructions that, when executed on a computer or processor, cause the computer or processor to perform the method described in any of the above embodiments.

[0231] The above-described apparatus embodiments correspond to the method embodiments and have the same technical effects. For detailed descriptions, please refer to the method embodiments. The apparatus embodiments are derived from the method embodiments; detailed descriptions can be found in the method embodiments section, and will not be repeated here. Those skilled in the art will understand that the accompanying drawings are merely schematic diagrams of one embodiment, and the modules or processes shown in the drawings are not necessarily essential for implementing this application.

[0232] Those skilled in the art will understand that the modules in the apparatus of the embodiments can be distributed in the apparatus of the embodiments as described in the embodiments, or they can be located in one or more devices different from this embodiment with corresponding changes. The modules of the above embodiments can be combined into one module, or they can be further divided into multiple sub-modules.

[0233] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of this application, and are not intended to limit them. Although this application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of this application.

Claims

1. A trajectory planning and tracking control method for an articulated vehicle, characterized in that, The articulated vehicle includes a tractor unit and a semi-trailer articulated with the tractor unit, and the method includes: The first planned trajectory of the tractor is searched using a heuristic search algorithm; For each first trajectory point in the first planned trajectory, based on the first trajectory point information, the non-time reference dynamic equation regarding the target angle between the tractor and the semi-trailer, and the geometric relationship between the tractor and the semi-trailer, the second trajectory point information corresponding to the first trajectory point information is determined, and the second planned trajectory of the semi-trailer is generated based on multiple second trajectory point information. The non-time reference dynamic equation is a discretized equation with the target angle as the state variable and the front wheel rotation angle of the tractor as the control variable, and the second trajectory point information is the trajectory point information of the semi-trailer. The semi-trailer path tracking control model is solved by taking the second planned trajectory as the desired trajectory of the semi-trailer, and the optimal solution of the semi-trailer path tracking control model is taken as the tracking control quantity of the semi-trailer. The semi-trailer path tracking control model is a path tracking control model with the optimization objective of minimizing the weighted average tracking error of the semi-trailer lateral tracking error, the tracking error of the semi-trailer yaw angle, and the tracking error of the target angle between the tractor and the semi-trailer. The tractor path tracking control model is solved by taking the first planned trajectory as the desired trajectory of the tractor, and the optimal solution of the tractor path tracking control model is taken as the tracking control quantity of the tractor. The tractor path tracking control model is a path tracking control model with the optimization objective of minimizing the weighted average tracking error of the tractor lateral tracking error, the tracking error of the tractor yaw angle, and the tracking error of the tractor front wheel steering angle.

2. The method according to claim 1, characterized in that, The basic action units of the heuristic search algorithm include spiral basic action units, circular arc basic action units, and straight line segment basic action units. The basic spiral motion unit includes a spiral line from a straight line segment to a spiral line with a first preset radius, a spiral line from a first target radius to a second target radius, a spiral line from a second target preset radius to a first target radius, and a spiral line from the first preset radius to a straight line segment. The first target radius and the second target radius are two adjacent preset radii among a plurality of preset radii set in a preset order, and the first target radius includes the first preset radius. The basic arc motion unit includes an arc with radii of each of the plurality of preset radii, which extends toward the target direction; The angular change is the same for each basic spiral motion unit and each basic circular arc motion unit. The process of searching the first planned trajectory of the tractor using a heuristic search algorithm includes: Based on the principle of continuous curvature of the planned trajectory, the planned trajectory of the tractor is obtained by heuristically searching by splicing the basic action units of the spiral, the basic action units of the circular arc and the basic action units of the straight line segment.

3. The method according to claim 1, characterized in that, The process of searching the first planned trajectory of the tractor using a heuristic search algorithm includes: The heuristic search algorithm is used to search for the first trajectory point information of the tractor at time k+1; The target angle at time k+1 is calculated based on the target angle at time k, the front wheel angle of the tractor at time k, the distance from the midpoint of the rear axle of the tractor to the articulation point, the distance from the articulation point to the midpoint of the rear axle of the semi-trailer, the distance from the midpoint of the front axle of the tractor to the midpoint of the rear axle, the first calculation step size, and the non-time reference dynamic equation. When the target angle at time k+1 is greater than or equal to a preset angle threshold, the heuristic search algorithm is used again to search for the first trajectory point information of the tractor at time k+1.

4. The method according to claim 3, characterized in that, The preset included angle threshold is determined based on the distance from the midpoint of the rear axle of the tractor to the hinge point, the distance from the hinge point to the midpoint of the rear axle of the semi-trailer, and the minimum turning radius of the tractor.

5. The method according to claim 4, characterized in that, The preset included angle threshold includes: Wherein, the β max The preset included angle threshold is represented by M1, which represents the distance from the midpoint of the rear axle of the tractor to the articulation point, and L2 represents the distance from the articulation point to the midpoint of the rear axle of the semi-trailer. R... min This indicates the minimum turning radius of the tractor unit.

6. The method according to claim 1, characterized in that, Based on the first trajectory point information, the non-time reference dynamic equation regarding the target angle between the tractor and the semi-trailer, and the geometric relationship between the tractor and the semi-trailer, the second trajectory point information corresponding to the first trajectory point information is determined, including: The target angle at time k+1 is calculated based on the target angle at time k, the front wheel angle of the tractor at time k, the distance from the midpoint of the rear axle of the tractor to the articulation point, the distance from the articulation point to the midpoint of the rear axle of the semi-trailer, the distance from the midpoint of the front axle of the tractor to the midpoint of the rear axle, the first calculation step size, and the non-time reference dynamic equation. For the same target time, based on the planned pose of the tractor at the target time and the geometric relationship between the tractor and the semi-trailer in the first trajectory point information, the planned pose of the semi-trailer at the target time is calculated. Based on other state information of the tractor at the target time and the geometric relationship between the tractor and the semi-trailer in the first trajectory point information, other state information of the semi-trailer at the target time is calculated. The other state information includes at least the planned vehicle speed.

7. The method according to claim 3 or 6, characterized in that, The non-time-referenced dynamic equations include: b k+1 =b k +hK2 in, Wherein, the β k+1 β k Let K1 and K2 represent the target angles at time (k+1) and time (k), respectively; let h represent the first calculation step size; let K1 and K2 represent the coefficients of the second-order Runge-Kutta integral method; and let δ... k L1 represents the turning angle of the front wheel of the tractor at time k, L2 represents the distance from the center of the front axle to the center of the rear axle of the tractor, M1 represents the distance from the articulation point to the midpoint of the rear axle of the semi-trailer, and M2 represents the distance from the midpoint of the rear axle of the tractor to the articulation point.

8. The method according to claim 1, characterized in that, The method further includes: Using the geometric relationship between the speeds of the tractor and the semi-trailer, and the planned speed of the tractor included in the first planned trajectory, the temporary planned speed of the semi-trailer is calculated. The predicted trajectory length of the semi-trailer is deduced based on the temporary planned speed and planning time of the semi-trailer. Using the association model of the tractor and the semi-trailer, the predicted trajectory length of the semi-trailer is converted into the predicted trajectory length of the tractor. Based on the predicted trajectory length of the tractor, the planned speed of the tractor in the first planned trajectory is corrected to obtain the corrected planned speed of the tractor.

9. The method according to claim 8, characterized in that, The method further includes: The planned speed of the tractor is adjusted using the sliding window method so that the planned speed of the tractor changes in advance. The planned vehicle speed after the sliding window method is smoothed by a constraint optimization method, and the smoothed planned vehicle speed of the tractor is obtained as the final planned vehicle speed. Using the geometric relationship between the speeds of the tractor and the semi-trailer, and the smoothed planned speed of the tractor, the smoothed planned speed of the semi-trailer is calculated as the final planned speed.

10. The method according to claim 1, characterized in that, The semi-trailer path tracking control model includes: Wherein, the ω f (k) represents the front wheel angular velocity of the semi-trailer to be tracked and controlled at time k, N represents time N, y2(k) and y2(k+1) represent the lateral position coordinates of the semi-trailer to be tracked and controlled at time k and time k+1, respectively, x2(k) and x2(k+1) represent the longitudinal position coordinates of the semi-trailer to be tracked and controlled at time k and time k+1, respectively, θ2(k) and θ2(k+1) represent the yaw angles of the semi-trailer to be tracked and controlled at time k and time k+1, respectively, and β k The y represents the included angle of the target to be tracked and controlled at time k. 2,ref (k) represents the expected lateral position coordinate of the semi-trailer at time k, where θ 2,ref (k) represents the expected yaw angle of the semi-trailer at time k, and β ref (k) represents the expected target angle of the semi-trailer at time k, Q2 represents the weight coefficient for the process points of the semi-trailer's planned trajectory, W2 represents the weight coefficient for the endpoint of the semi-trailer's planned trajectory, v2 represents the planned speed of the semi-trailer, Δt represents the second calculation step size, L2 represents the distance from the articulation point to the midpoint of the semi-trailer's rear axle, and β max This indicates the preset included angle threshold.

11. The method according to any one of claims 1-6 and 8-10, characterized in that, The tractor path tracking control model includes: Wherein, δ(k) represents the front wheel steering angle of the tractor to be tracked and controlled at time k, N represents time N, y1(k) and y1(k+1) represent the lateral position coordinates of the tractor to be tracked and controlled at time k and time k+1, respectively, x1(k) and x1(k+1) represent the longitudinal position coordinates of the tractor to be tracked and controlled at time k and time k+1, respectively, θ1(k) and θ1(k+1) represent the yaw angle of the tractor to be tracked and controlled at time k and time k+1, respectively. 1,ref (k) represents the desired lateral position coordinate of the tractor at time k, where θ 1,ref (k) represents the expected yaw angle of the tractor at time k, and the δ ref (k) represents the desired target angle of the tractor at time k, Q1 represents the weight coefficient for the process points of the tractor's planned trajectory, W1 represents the weight coefficient for the endpoint of the tractor's planned trajectory, v1 represents the planned speed of the tractor, Δt represents the second calculation step size, L1 represents the distance from the center of the front axle to the center of the rear axle of the tractor, and δ max This indicates the preset front wheel steering angle threshold.

12. A trajectory planning and tracking control device for an articulated vehicle, characterized in that, The articulated vehicle includes a tractor unit and a semi-trailer articulated with the tractor unit, and the device includes: The first trajectory planning module is used to search the first planned trajectory of the tractor using a heuristic search algorithm; The second trajectory planning module is used to determine the second trajectory point information corresponding to each first trajectory point information in the first planned trajectory, based on the first trajectory point information, the non-time reference dynamic equation about the target angle between the tractor and the semi-trailer, and the geometric relationship between the tractor and the semi-trailer, and to generate the second planned trajectory of the semi-trailer based on multiple second trajectory point information. The non-time reference dynamic equation is a discretized equation with the target angle as the state variable and the front wheel rotation angle of the tractor as the control variable, and the second trajectory point information is the trajectory point information of the semi-trailer. The first tracking control module is used to solve the semi-trailer path tracking control model by taking the second planned trajectory as the desired trajectory of the semi-trailer, and to take the optimal solution of the semi-trailer path tracking control model as the tracking control quantity of the semi-trailer. The semi-trailer path tracking control model is a path tracking control model with the optimization objective of minimizing the weighted average tracking error of the semi-trailer lateral tracking error, the tracking error of the semi-trailer yaw angle, and the tracking error of the target angle between the tractor and the semi-trailer. The second tracking control module is used to solve the tractor path tracking control model by taking the first planned trajectory as the desired trajectory of the tractor, and to take the optimal solution of the tractor path tracking control model as the tracking control quantity of the tractor. The tractor path tracking control model is a path tracking control model with the optimization objective of minimizing the weighted average tracking error of the tractor lateral tracking error, the tracking error of the tractor yaw angle, and the tracking error of the tractor front wheel steering angle.

13. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the program is executed by the processor, it implements the method as described in any one of claims 1-11.

14. An electronic device, characterized in that, The electronic device includes: One or more processors; The processor is coupled to a storage device for storing one or more programs; When the one or more programs are executed by the one or more processors, the electronic device performs the method as described in any one of claims 1-11.

15. An articulated vehicle, characterized in that, The articulated vehicle includes the device as described in claim 12, or the electronic device as described in claim 14.

Images