Parking path planning method and device and computer readable storage medium

Abstract

The invention provides a parking path planning method and device and a computer readable storage medium, and the method comprises the steps: setting a target coordinate system which is established by taking a target position of a target state during parking of a mount as an original point, obtaining an initial state during parking of the mount under the target coordinate system, and obtaining a parking path based on the initial state of the mount, the target state and a preset constraint condition; the optimization objective function established according to the speed, the acceleration and the hinge angle is solved under the target coordinate system, the motion trails of the vehicle in different states are accurately predicted so as to obtain the mounting motion trail when the mounting reaches the target state under the scene without the structured road information, meanwhile, the parameters such as the speed and the acceleration of vehicle driving are considered, and the accuracy of the mounting motion trail is improved. Therefore, the vehicle has smoothness and drivability when driving based on the solved track. And then the mounting motion trail is converted into a world coordinate system where the tractor is located, the motion trail of the tractor is obtained, and semi-trailer tractor parking in the scene without the structured road information is achieved.

Description

Technical Field

[0001] The present application relates to the technical field of tractor vehicles, and in particular to a parking path planning method, device and computer-readable storage medium. Background Art

[0002] Tractor trailers have great advantages in freight volume and transportation costs, and can provide higher transportation efficiency. Therefore, tractor trailers play an important role in the transportation industry and undertake more tasks in ports, factories and other scenarios.

[0003] In operating scenarios such as ports, tasks often require the load carried by the tractor trailer to complete the docking according to the issued direction and position instructions. However, current research on vehicle parking in fixed parking spaces mainly focuses on small vehicles such as passenger cars. Small vehicles usually have a simpler kinematic model, are small in size and highly flexible, and have great differences in structure and motion characteristics from tractor trailers with loads. Therefore, the kinematic model considered is not applicable to tractor trailers with loads. The path planning scheme for tractor trailers with loads is implemented based on a search method under structured roads. Since parking tasks in scenarios such as ports lack road structure information, the path planning methods currently available on the market for tractor trailers with loads cannot be applied to scenarios such as ports where there is no road information. In addition, the search-based method is limited in the smoothness and drivability of the path.

[0004] Therefore, it is urgent to design a new path planning method to solve the problem of docking a semi-trailer tractor with a load in the absence of a road structure. Summary of the invention

[0005] To overcome the problems existing in the related art, this specification provides a parking path planning method, device and computer-readable storage medium.

[0006] According to a first aspect of an embodiment of this specification, a parking path planning method is provided, which is applied to a semi-trailer tractor, wherein the semi-trailer tractor comprises a tractor and a load, and the method comprises:

[0007] According to a parking path planning method provided by the present application, an initial state of the mount when parking in a target coordinate system is obtained, and the target coordinate system is established with a target position in a target state when the mount is parked as an origin;

[0008] Based on the initial state, the target state and the preset constraint conditions, solving the optimization objective function about the speed, acceleration and the articulation angle between the mount and the tractor in the target coordinate system to obtain the mount motion trajectory;

[0009] Convert the mounting motion trajectory into the world coordinate system where the tractor is located to obtain the tractor motion trajectory;

[0010] The semi-trailer tractor is controlled to travel based on the motion trajectory of the tractor so that the load is parked based on the target state.

[0011] According to a parking path planning method provided by the present application, the obtaining of the initial state of the mounted parking space in the target coordinate system includes:

[0012] Obtaining the current state of the mount in the world coordinate system where the tractor is located and the target state to be achieved when parking;

[0013] Establishing a target coordinate system with the target position in the target state as the coordinate origin;

[0014] Based on the current state, an initial state of the mount in the target coordinate system is determined, wherein the initial state includes an initial position as a starting point of the planned motion trajectory of the mount.

[0015] According to a parking path planning method provided by the present application, the constraint conditions include kinematic constraints of the mount, and the kinematic constraints include constraints on speed, acceleration, angular velocity, and an articulation angle between the tractor and the mount.

[0016] According to a parking path planning method provided by the present application, the constraint conditions further include position constraints and size constraints of the tractor and the mount;

[0017] The position constraint is determined according to the initial state and is used to limit the driving range of the semi-trailer tractor.

[0018] According to a parking path planning method provided by the present application, based on the initial state, the target state and the preset constraints, the optimization objective function about the speed, acceleration and the articulation angle between the mount and the tractor is solved in the target coordinate system to obtain the mount motion trajectory, including:

[0019] Call the solver;

[0020] In the target coordinate system, based on the initial state, the target state and preset constraints, a convex optimization algorithm is used to solve the optimization objective function regarding speed, acceleration and the articulation angle between the mount and the tractor to obtain the mount motion trajectory.

[0021] According to a parking path planning method provided by the present application, after obtaining the mounting motion trajectory, the method further includes:

[0022] According to the forward / backward changes of the mount during driving, the motion trajectory of the mount is segmented to obtain multiple segmented initial motion trajectories.

[0023] According to a parking path planning method provided by the present application, after segmenting the mounting motion trajectory according to the forward / reverse change of driving to obtain multiple segmented initial motion trajectories, the method further includes:

[0024] The initial motion trajectory after multi-segment segmentation is interpolated to obtain the optimized mounting motion trajectory.

[0025] According to a parking path planning method provided by the present application, the initial motion trajectory includes a plurality of trajectory points, each of which has trajectory information including speed, direction and time.

[0026] The interpolation of the initial motion trajectory after multiple segmentation to obtain the optimized mounting motion trajectory includes:

[0027] The following steps are performed on the initial motion trajectory after two adjacent segments are divided:

[0028] Determine the initial and final accelerations and the initial and final steering angular velocities of the two trajectory points according to the trajectory information of the two trajectory points in the initial and final states in the adjacent initial motion trajectory;

[0029] According to the initial and final accelerations and the initial and final steering angular velocities, a trajectory point corresponding to the velocity zero point is determined as a target interpolation point;

[0030] Based on the target interpolation point, re-segment the segmented initial motion trajectory to obtain a segmented target motion trajectory;

[0031] Interpolate multiple segments of the target motion trajectory to obtain an optimized mounting motion trajectory.

[0032] According to a parking path planning method provided by the present application, the interpolation of multiple segments of the target motion trajectory to obtain an optimized mounting motion trajectory includes:

[0033] The following steps are performed for each segmented target motion trajectory:

[0034] Determining the relative acceleration and relative steering angular velocity between adjacent trajectory points according to trajectory information of adjacent trajectory points in the current target motion trajectory;

[0035] Based on the motion model of the constant relative acceleration and the constant relative steering angular velocity, the trajectory points to be interpolated are determined according to a preset interpolation interval method;

[0036] Interpolating the current target motion trajectory based on the trajectory points to be interpolated;

[0037] After interpolating each segmented target motion trajectory, an optimized mounting motion trajectory is obtained.

[0038] According to a parking path planning method provided by the present application, the preset interpolation interval mode includes a time interval and a distance interval.

[0039] According to a parking path planning method provided by the present application, the step of converting the mounting motion trajectory into the world coordinate system where the tractor is located to obtain the tractor motion trajectory includes:

[0040] The optimized mounting motion trajectory is converted into the world coordinate system where the tractor is located to obtain multiple segments of the tractor motion trajectory.

[0041] According to a parking path planning method provided by the present application, the semi-trailer tractor is controlled to travel based on the motion trajectory of the tractor so that the mount completes parking based on the target state, including:

[0042] Control the semi-trailer tractor by combining each segment of the tractor motion trajectory with the gear information, so that the load is parked based on the target state;

[0043] The gear information is associated with the forward / reverse change during the mounted driving, and is used to determine the driving direction of the semi-trailer tractor.

[0044] According to a parking path planning method provided by the present application, the step of converting the mounting motion trajectory into the world coordinate system where the tractor is located to obtain the tractor motion trajectory includes:

[0045] Determining, based on the articulation angle between the mount and the tractor, a motion offset of the tractor relative to the mount in the target coordinate system;

[0046] The mounting motion trajectory and the motion offset are converted into the world coordinate system where the tractor is located, and the position of each trajectory point in the mounting motion trajectory after conversion is added to the motion offset, and the obtained new trajectory points form the tractor motion trajectory.

[0047] According to a parking path planning method provided by the present application, the semi-trailer tractor is controlled to travel based on the motion trajectory of the tractor so that the mount completes parking based on the target state, including:

[0048] During the driving process of the semi-trailer tractor controlled by the tractor motion trajectory, determining the minimum distance between the current position of the mount and the current segment of the mount motion trajectory based on a set detection frequency;

[0049] When the minimum distance is less than a calibrated threshold, continue to control the semi-trailer tractor to travel based on the tractor motion trajectory, so that the mount completes parking based on the target state;

[0050] When the minimum distance is greater than or equal to the calibration threshold, the step of obtaining the initial state of the mount when it is parked in the target coordinate system is re-executed with the current position as the starting point to update the mount motion trajectory.

[0051] According to a parking path planning method provided by the present application, the method further includes:

[0052] When the minimum distance is greater than or equal to the calibration threshold, determining whether the trajectory point corresponding to the minimum distance is the end point of the mounting motion trajectory;

[0053] If not, taking the current position as the starting point, the step of obtaining the initial state of the mount when it is parked in the target coordinate system is re-executed to update the mount motion trajectory.

[0054] The present application also provides a parking path planning device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, the parking path planning method described above is implemented.

[0055] The present application also provides a computer-readable storage medium having a computer program stored thereon, and when the computer program is executed by a processor, the parking path planning method as described in any one of the above is implemented.

[0056] Compared with the current problem of semi-trailer tractors with mounts being difficult to park under conditions without road structures, the parking path planning method, device and computer-readable storage medium in the embodiments of this specification, by setting a target coordinate system established with the target state and target position of the mount when parking as the origin, obtaining the initial state of the mount when parking in the target coordinate system, solving the optimization objective function established according to speed, acceleration and articulation angle in the target coordinate system based on the initial state of the mount, the target state and the preset constraints, the optimization objective function considers the mount structure information including the mount position and articulation angle, and can more accurately predict the motion trajectory of the vehicle in different states, so as to obtain the motion trajectory of the mount when the mount reaches the target state in the unstructured road information scene, and at the same time considers the parameters such as the speed and acceleration of the vehicle, so that the vehicle has smoothness and drivability when driving based on the solved trajectory. Then, the motion trajectory of the mount is converted to the world coordinate system where the tractor is located, and the motion trajectory of the tractor is obtained, so as to realize the parking of the semi-trailer tractor in the unstructured road information scene.

[0057] It is to be understood that the foregoing general description and the following detailed description are exemplary and explanatory only and are not restrictive of the present specification. BRIEF DESCRIPTION OF THE DRAWINGS

[0058] The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the specification and, together with the description, serve to explain the principles of the specification.

[0059] Figure 1 is a flowchart of a parking path planning method according to an exemplary embodiment of the present specification;

[0060] Figure 2 is a variable schematic diagram based on a kinematic model according to an exemplary embodiment of this specification;

[0061] Figure 3 is a schematic diagram of a coordinate conversion process according to an exemplary embodiment of the present specification;

[0062] Figure 4 is another flow chart of a parking path planning method according to an exemplary embodiment of the present specification;

[0063] Figure 5 is a flow chart of a trajectory reuse process in a parking path planning method according to an exemplary embodiment of the present specification;

[0064] Figure 6 is a schematic diagram of a parking path planning device according to an exemplary embodiment of the present specification;

[0065] Figure 7 This is a schematic block diagram of a parking path planning device according to an exemplary embodiment of the present specification. DETAILED DESCRIPTION

[0066] Here, the technical solutions in the embodiments (or "implementations") of the present application will be described clearly and completely in conjunction with the accompanying drawings. When the following description refers to the drawings, unless otherwise indicated, the same numbers in different drawings represent the same or similar elements.

[0067] If there are terms involving directional indications or positional relationships in the embodiments of the present application (such as up, down, left, right, front, back, inside, outside, top, bottom, center, vertical, horizontal, longitudinal, transverse, length, width, counterclockwise, clockwise, axial, radial, circumferential, etc.), such terms are only used to explain the relative positional relationship, movement, etc. between the components under a certain specific posture (as shown in the accompanying drawings); if the specific posture changes, the directional indication or positional relationship will also change accordingly. In addition, the terms "first", "second", etc. involved in the embodiments of the present application are only used for the purpose of convenience of description and cannot be understood as indicating or implying relative importance.

[0068] The present application provides a parking path planning method, device and computer readable storage medium. The present application is described in detail below in conjunction with the accompanying drawings. In the absence of conflict, the features of the following embodiments and implementations can be combined with each other.

[0069] Tractor trailers have high advantages in freight volume and transportation costs, and can provide high transportation efficiency. Therefore, tractor trailers play an important role in the transportation industry and undertake many tasks in ports, factories and other scenarios. For example, in port operations, tasks often require the semi-trailer tractor to dock according to the issued direction and position instructions. However, current research on vehicle parking in fixed parking spaces mainly focuses on small vehicles such as passenger cars. Since small vehicles have simpler kinematic models, they are small in size and highly flexible, and have great differences in structure and motion characteristics from semi-trailer tractors with mounts. Therefore, the kinematic model considered is not applicable to semi-trailer tractors with mounts.

[0070] However, the current path planning scheme for semi-trailer tractors is implemented based on a search method in the case of structured roads (for example, the initial path is obtained through the A* search method, and then optimized based on the RS curve and MPC method to obtain a smoother reversing trajectory). However, parking in scenes such as ports lacks road structure information. Therefore, the current path planning scheme for semi-trailer tractors is not suitable for this scenario. In addition, the search-based scheme is also subject to certain limitations in the smoothness and drivability of path planning.

[0071] In order to solve the above technical problems, this specification provides a parking path planning method.

[0072] By introducing a kinematic model with an articulation angle between the mount and the vehicle, a smooth trajectory is obtained based on a convex optimization method to achieve the trajectory planning of a semi-trailer vehicle with a trailer entering a parking space. The semi-trailer tractor described in this paper consists of a tractor that provides power and a mount with cargo. The articulation angle is a parameter that describes the relative angle relationship between the tractor and the mount in the semi-trailer tractor. The kinematic characteristics of the semi-trailer tractor with a mount can be better described by considering the articulation angle. In path planning, changes in the articulation angle will affect the driving trajectory and stability of the tractor. Therefore, taking it into consideration improves the accuracy and reliability of planning.

[0073] Specifically, based on the convex optimization technology, the geometric dimensions of the tractor and the mount and the kinematic model with the added articulation angle are taken into account, which can realize the parking alignment of the mount, and optimize the acceleration, jerk, angular velocity, etc. during the parking process to improve the smoothness and drivability of the generated trajectory. This can solve the problem of a semi-trailer tractor with a mount parking in a specified position and orientation without a road structure, and at the same time constrain the articulation angle between the mount and the vehicle to avoid folding.

[0074] It should be noted that the parking path planning method in this article is not only applicable to semi-trailer tractors, but also to any vehicle with a mount, without specific limitation here. The following is an explanation using a semi-trailer tractor as an example.

[0075] Figure 1 : is a flow chart of a parking path planning method provided in an embodiment of this specification, comprising the following steps:

[0076] S100, obtaining an initial state of the mount when it is parked in a target coordinate system, wherein the target coordinate system is established with a target position in a target state when the mount is parked as an origin.

[0077] When a semi-trailer tractor with a mount is parking in a scenario without road structure information, the mount needs to be parked in a specified position and orientation. At this time, the tractor and the mount position are transferred from the world coordinate system where the tractor is located to the target coordinate system where the mount is located, to ensure that the trajectory generation of the mount from the starting point to the final position can be completed in a scenario without structured road information.

[0078] Specifically, the target state of the mount required to stop is determined in the world coordinate system established with the current position of the tractor as the origin, and the target state includes the target position and the target orientation. A target coordinate system is established with the target position as the origin, and the initial state of the mount in the target coordinate system is calculated, and the initial state includes the initial position and the initial orientation. In other words, the position and orientation corresponding to the current position and orientation of the mount in the target coordinate system are calculated, and the initial position in the target coordinate system is used as the starting point in the mount path planning, and the end point is the target position.

[0079] In some embodiments, the obtaining of the initial state of the mount stop position in the target coordinate system includes:

[0080] S110, obtaining the current state of the mount in the world coordinate system where the tractor is located and the target state to be achieved when parking.

[0081] S120, establishing a target coordinate system with the target position in the target state as a coordinate origin.

[0082] S130, based on the current state, determining an initial state of the mount in the target coordinate system, wherein the initial state includes an initial position as a starting point of the planned motion trajectory of the mount.

[0083] Through a certain coordinate transformation, the semi-trailer tractor in the world coordinate system is transformed into the target coordinate system established with the mount as the center, and the current position of the mount is used as the starting position of the path planning, and the target position when the mount is docked is used as the end position of the path planning, providing a clear reference point for trajectory planning, and then planning the movement path of the mount.

[0084] S200, based on the initial state, the target state and preset constraints, solving the optimization objective function regarding the speed, acceleration and the articulation angle between the mount and the tractor in the target coordinate system to obtain a motion trajectory of the mount.

[0085] Through the convex optimization algorithm, the optimal driving path can be determined based on the initial position and initial orientation, the target position and target orientation, and the kinematic characteristics of the tractor and the mount. By adding kinematic constraints such as vehicle speed, acceleration, angular velocity, and articulation angle into the optimization process, and taking variables such as acceleration and jerk as optimization targets, the smoothness of the generated trajectory is guaranteed.

[0086] The preset constraints also include position constraints, size constraints of the tractor and the mount, and consciously take into account the parking alignment of the mount.

[0087] The position constraint is determined according to the initial state and is used to limit the driving range of the semi-trailer tractor. Specifically, the position constraint is calculated according to the initial position, and the position constraint can limit the vehicle to travel within a specific area or ensure that the vehicle meets certain conditions at a specific position. The position constraint is calculated based on factors such as the initial position, the target position, and the surrounding environment.

[0088] In trajectory planning, in addition to position constraints, other types of constraints need to be set, such as kinematic transportation of mounts, including speed constraints, acceleration constraints, angular velocity constraints, and articulation angle constraints. These constraints ensure that the generated trajectory meets the kinematic characteristics of the vehicle and the actual driving requirements. By comprehensively considering various constraints, a reasonable optimization objective function is established to provide a clear constraint range for subsequent calls to the solver.

[0089] In this embodiment, by adding constraints such as articulation angle constraints and acceleration during the optimization process, and adding the function of determining appropriate position constraints based on the positions of the initial state and the target state, these constraints are effectively processed through the convex optimization method to ensure that the generated trajectory meets all constraint requirements. By setting reasonable constraints, the smoothness and drivability of the generated trajectory can be guaranteed, while improving the efficiency and accuracy of path planning.

[0090] In summary, the kinematic relationship between the mounting size, position, articulation angle and vehicle is introduced into the kinematic model of the semi-trailer tractor to ensure that the algorithm can be applied to the path planning of the tractor. The constraints of vehicle speed, acceleration, angular velocity and articulation angle are added to the optimization process, and variables such as acceleration and jerk are used as optimization targets to ensure the smoothness of the generated trajectory.

[0091] As an example, the following parameters and variables of the vehicle and the mount are introduced in the path planning process. The relationship between the variables is as follows: Figure 2 As shown, Figure 2 Schematic diagram of variables based on the kinematic model.

[0092] The x coordinate of the center of the rear axle after mounting

[0093] The y coordinate of the center of the rear axle

[0094] Mounting direction θ

[0095] Articulation angle β between vehicle and load

[0096] Vehicle speed v

[0097] The speed of the rear wheel center in the x direction

[0098] The speed of the rear wheel center in the y direction

[0099] Angular velocity of the mount

[0100] Angular velocity of the articulation angle

[0101] Vehicle acceleration

[0102] Vehicle steering angle α and steering angle acceleration

[0103] In the optimization process, constraints are introduced based on the kinematic model. The constraints obtained by the forward Euler method are as follows:

[0104]

[0105] In the optimization process, the articulation angle, speed, and acceleration are selected as optimization targets, and the optimization objective function is:

[0106]

[0107] It should be noted that other variables can be added as optimization indicators and the optimization weights can be modified.

[0108] Next, the solver is called, the initial state, target state and preset conditions are added to the problem, and the optimization objective function is solved using the convex optimization algorithm to obtain the solution result including the mount motion trajectory that satisfies all constraints and the optimization objective function. The solver is a tool for solving optimization problems, which can find the optimal solution based on the given objective function and constraints.

[0109] The solver usually returns a trajectory solution containing the position, speed, acceleration and other information of the vehicle at different times, that is, the mounted motion trajectory. The solution result is updated to the current message frame for subsequent processing and planning of the tractor's motion trajectory. Among them, the message frame is a data structure used to transmit information in the system, which contains the status information of the semi-trailer tractor, trajectory planning results and other contents. By updating the stream frame, other modules in the system can obtain the latest trajectory planning results in time and control the vehicle to drive to the specified position.

[0110] In this embodiment, the kinematic model can take into account parameters such as the speed, acceleration, steering angular velocity, and the change law of the articulation angle of the semi-trailer tractor, so as to better control the movement of the semi-trailer tractor during the path planning process and ensure that the generated trajectory conforms to the kinematic characteristics of the vehicle.

[0111] And by directly optimizing according to the kinematic model, compared with the method of determining the initial trajectory based on search and then smoothing, the search process is reduced and the uncertainty of the algorithm is reduced.

[0112] S300: Convert the mounting motion trajectory into the world coordinate system where the tractor is located to obtain the tractor motion trajectory.

[0113] Since the motion trajectory of the mount is calculated in the target coordinate system where the mount is located, and the actual output trajectory needs to be expressed in the world coordinate system where the tractor is located, coordinate conversion is required to convert the motion trajectory of the mount from the target coordinate system to the world coordinate system to obtain the motion trajectory of the tractor. The motion trajectory of the tractor can be used to control the driving of the vehicle so that the semi-trailer tractor can accurately reach the designated location according to the planned trajectory.

[0114] The specific conversion process is as follows: Figure 3 The step of converting the mounting motion trajectory into the world coordinate system where the tractor is located to obtain the tractor motion trajectory includes:

[0115] S310: Determine, based on the articulation angle between the mount and the tractor, a movement offset of the tractor relative to the mount in the target coordinate system.

[0116] The motion offset described in this article refers to the position offset of the tractor (or the center of the rear axle of the tractor) relative to the position of the mount in the target coordinate system. The offset can be represented by a two-dimensional or three-dimensional vector, including displacement components in the direction of each coordinate axis. By determining the motion offset, the position and posture of the semi-trailer tractor can be more accurately grasped for precise control and planning.

[0117] In the planning of the motion trajectory of the mount, the articulation angle between the tractor and the mount is introduced. Since the distance from the mount to the articulation point corresponding to the articulation angle and the distance from the tractor to the junction point are constant and determined by the geometric structure of the semi-trailer tractor, the change in the motion offset is caused by the change in the articulation angle. Then, the corresponding motion offset is determined according to the articulation angle at each moment obtained in the path planning.

[0118] S320, converting the mounting motion trajectory and the motion offset into the world coordinate system where the tractor is located, and adding the position of each trajectory point in the mounting motion trajectory after conversion to the motion offset, and the obtained new trajectory points form the tractor motion trajectory.

[0119] Convert the mount position from the target coordinate system to the world coordinate system so that subsequent calculations and analysis can be performed in a unified coordinate system. That is, convert the mount motion trajectory to the world coordinate system.

[0120] Next, according to the conversion relationship between the target coordinate system and the world coordinate system, the motion offset of the tractor is also converted to the world coordinate system to determine the motion trajectory of the tractor in the world coordinate system.

[0121] Specifically, the position of the tractor (or the rear axle center of the tractor) in the world coordinate system is obtained by adding the mounting position in the mounting motion trajectory in the world coordinate system and the motion offset corresponding to the position, thereby forming the tractor motion trajectory.

[0122] S400: Controlling the semi-trailer tractor to travel based on the motion trajectory of the tractor, so that the load is parked based on the target state.

[0123] The motion trajectory of the tractor includes the trajectory of the expected position and time information of the tractor (or the center of the rear axle of the tractor), and describes the expected position of the semi-trailer tractor at different time points. The vehicle system of the semi-trailer tractor can determine the driving speed and acceleration at different times based on the time information to ensure that the semi-trailer tractor can reach the specified position on time according to the trajectory. At this time, the mounting is based on the target position and target orientation in the target state to complete the parking. Furthermore, the vehicle control system can calculate the control instructions of the vehicle at different times, such as steering angle, throttle opening and braking force, based on the position and time information in the planned trajectory, and continuously adjust the control parameters of the semi-trailer tractor to make it travel along the planned trajectory, and finally achieve accurate parking. At the same time, the control system can also adjust and optimize the planned tractor motion trajectory in real time according to the actual driving conditions to improve the accuracy and reliability of parking.

[0124] The present application provides a parking path planning method, device and computer-readable storage medium. Compared with the current problem that it is difficult to solve the parking of a semi-trailer tractor with a mount under the condition of no road structure, a target coordinate system is set up with the target state and target position of the mount when it is parked as the origin, and the initial state of the mount when it is parked in the target coordinate system is obtained. Based on the initial state of the mount, the target state and the preset constraint conditions, the optimization objective function established according to the speed, acceleration and articulation angle is solved in the target coordinate system. The optimization objective function considers the mount structure information including the mount position and articulation angle, and can more accurately predict the motion trajectory of the vehicle in different states, so as to obtain the motion trajectory of the mount when the mount reaches the target state in the unstructured road information scene, and at the same time considers the parameters such as the speed and acceleration of the vehicle, so that the vehicle has smoothness and drivability when driving based on the solved trajectory. Thereafter, the motion trajectory of the mount is converted to the world coordinate system where the tractor is located, and the motion trajectory of the tractor is obtained, so as to realize the parking of the semi-trailer tractor in the unstructured road information scene.

[0125] Based on the above first embodiment, a second embodiment of a parking path planning method is proposed. Figure 4 Another flowchart of the parking path planning method is shown in FIG.

[0126] In order to better adapt to the actual driving needs of the semi-trailer tractor and to facilitate the separate processing and optimization of the trajectories under different driving directions, the solved mounting motion trajectory is divided into multiple trajectories.

[0127] Specifically, after obtaining the mounting motion trajectory in S200, the method further includes:

[0128] According to the forward / backward changes of the mount during driving, the mount motion trajectory is segmented to obtain multiple segmented initial motion trajectories. The initial motion trajectory includes multiple trajectory points, each of which has trajectory information including speed, direction and time.

[0129] The segmented initial motion trajectory is further used to execute the steps after S300, that is, multiple segments of tractor motion trajectory are obtained according to the segmented initial motion trajectory, and are sent to the vehicle end for segmented execution.

[0130] In some embodiments, the controlling the semi-trailer tractor to travel based on the tractor motion trajectory so that the mount completes parking based on the target state includes:

[0131] Control the semi-trailer tractor by combining each segment of the tractor motion trajectory with the gear information, so that the load is parked based on the target state;

[0132] The gear information is associated with the forward / reverse change during the mounted driving, and is used to determine the driving direction of the semi-trailer tractor.

[0133] Specifically, multiple segments of trajectories are combined with gear information D / R for output, where the gear information indicates the driving gear of the semi-trailer tractor, D indicates the forward gear, and R indicates the reverse gear. The driving direction of the semi-trailer tractor can be determined according to the gear information, and the corresponding trajectory segment can be selected for output. The output trajectory can be used to control the driving of the semi-trailer tractor so that it can accurately reach the specified position according to the planned trajectory.

[0134] In other embodiments, in order to make the planned motion trajectory smoother and more drivable, each segment of the mount motion trajectory is interpolated to increase the number of trajectory points. Interpolation is a method of inserting new data points between known data points.

[0135] Specifically, the initial motion trajectory after multiple segmentation is interpolated to obtain the optimized mounting motion trajectory.

[0136] The generated mounting motion trajectory for reversing contains multiple forward-backward-forward trajectories. In order to complete the segmentation and interpolation of the trajectory, a motion model based on constant steering angular velocity and constant acceleration is used for interpolation, which can quickly generate the interpolated trajectory, improve the interpolation efficiency, and meet the real-time requirements.

[0137] Since the aforementioned segmentation of the mount motion trajectory is achieved through the forward / reverse driving direction, it can be specifically determined by the speed direction of each trajectory point, and what is obtained are two adjacent trajectory points where the speed direction changes. In order to improve the accuracy during the actual vehicle control execution, it is necessary to determine the specific position where the vehicle shifts gears, which is the position corresponding to the speed zero point. Among them, this position may be at one of the two trajectory points where the driving direction changes, or it may be at a position in the area between the two trajectory points, and this position is not the solved trajectory point on the mount motion trajectory obtained by solving the optimization objective function mentioned above. Therefore, it is necessary to further optimize the initial motion trajectory after segmentation to obtain the target motion trajectory after segmentation, which is used to clearly control the forward / reverse trajectory of the vehicle.

[0138] Therefore, when interpolating the initial motion trajectory after segmentation based on the mounting motion trajectory, two processes are performed: interpolation of the position on the trajectory at the zero velocity point and interpolation of each segment of the optimized target motion trajectory.

[0139] The process of interpolating the position on the trajectory at the zero velocity point is as follows:

[0140] The interpolation of the initial motion trajectory after multiple segmentation to obtain the optimized mounting motion trajectory includes:

[0141] The following steps are performed on the initial motion trajectory after two adjacent segments are divided:

[0142] Step a1, determining the initial and final accelerations and initial and final steering angular velocities of the two trajectory points according to the trajectory information of the two trajectory points in the initial and final states in the adjacent initial motion trajectory; wherein the two trajectory points in the initial and final states refer to the last trajectory point in the previous initial motion trajectory and the first trajectory point in the next initial motion trajectory.

[0143] The aforementioned mounting motion trajectory contains parameter information such as speed, acceleration, direction, time, etc. of each trajectory point. Therefore, the speed, direction and relative time information of each trajectory point of each initial motion trajectory can be directly obtained according to the segmented initial motion trajectory.

[0144] Next, the acceleration and steering angular velocity are calculated by averaging the speed, direction and relative time of the two trajectory points at the start and end states. Specifically, the acceleration is obtained by dividing the speed difference of the two trajectory points by the relative time, which is used to represent the speed change rate of the mount when traveling in the interpolation interval; the steering angular velocity is obtained by dividing the direction difference of the two trajectory points by the relative time, which is used to represent the steering speed of the mount in the interpolation interval. These two parameters are crucial to determining the motion state of the mount in the interpolation interval.

[0145] Step a2, determining the trajectory point corresponding to the zero velocity point as the target interpolation point according to the initial and final accelerations and the initial and final steering angular velocities.

[0146] The zero speed point refers to the moment when the speed of the mount is zero within the interpolation interval. The time of the zero speed point is determined by analyzing the relationship between acceleration and speed. Then, based on the time of the zero speed point and the known initial and final states, the state quantities of the mount at the zero speed point, such as position, orientation, etc., are solved.

[0147] Step a3, re-segmenting the segmented initial motion trajectory based on the target interpolation point to obtain the segmented target motion trajectory, wherein the target interpolation point is the position and time when the mount is transformed forward / backward.

[0148] Step a4, interpolating multiple segments of the target motion trajectory to obtain an optimized mounting motion trajectory.

[0149] By segmenting again, the obtained mounting motion trajectory is more accurate and refined, and is used to control the vehicle's driving subsequently.

[0150] The process of interpolating each optimized target motion trajectory is as follows:

[0151] The interpolating multiple segments of the target motion trajectory to obtain an optimized mounting motion trajectory includes:

[0152] The following steps are performed for each segmented target motion trajectory:

[0153] Step b1, according to the trajectory information of the adjacent trajectory points in the current target motion trajectory, determine the relative acceleration and relative steering angular velocity between the adjacent trajectory points. The same as the above-mentioned method of interpolating between the two trajectory points in the initial and final states of the adjacent segments of the initial motion trajectory, this process is for the two adjacent trajectory points in each segment of the target motion trajectory.

[0154] Step b2, based on the motion model of the constant relative acceleration and the constant relative steering angular velocity, determine the trajectory points to be interpolated according to a preset interpolation interval method.

[0155] The motion model based on constant relative acceleration and constant relative steering angular velocity has certain regularity, making the motion trajectory of the mount easier to control and predict.

[0156] In some embodiments, the number of solution points in the optimization process is selected according to the distance between the start and end positions to reduce the amount of calculation in the optimization process. For example, when the distance is close, fewer solution points are selected according to the estimated duration. It can be understood that when the distance between the start and end positions is close, the path of the vehicle is relatively short and the change of the trajectory is relatively simple. In this case, if more solution points are selected, the amount of calculation of the algorithm will increase, but the optimization effect of the trajectory will be limited. For another example, when the distance between the start and end positions is far, the path of the semi-trailer tractor is long and the change of the trajectory may be more complicated. At this time, more solution points are needed to more accurately describe the trajectory to ensure that the generated trajectory meets actual needs.

[0157] This method is used to improve solution efficiency and reduce algorithm calculation amount.

[0158] As an example, the interpolation position is determined according to the time interval / distance interval, that is, a new trajectory point is inserted every certain time or distance to improve the accuracy of the trajectory.

[0159] The following uses the time interval as an example for explanation. The processing methods corresponding to the distance interval are basically the same and will not be repeated here.

[0160] According to the known velocity, acceleration and time of the trajectory point, based on the set time interval, the position corresponding to each time interval is calculated, and the position is used as the trajectory point to be interpolated. Specifically:

[0161] x k+1 =x k +∫vcosθdt

[0162] y k+1 =y k +∫vsinθdt

[0163] θ k+1 =θ k +∫wdt

[0164] v k+1 =v k +∫adt

[0165] After the integral operation, the position coordinates of the newly inserted trajectory points during interpolation are as follows:

[0166]

[0167]

[0168] It should be noted that the time interval may also be determined according to the time between adjacent trajectory points in the target motion trajectory and the number of inserted trajectory points.

[0169] Step b3, interpolating the current target motion trajectory based on the trajectory points to be interpolated.

[0170] Based on the velocity, acceleration and position coordinates of the trajectory points, the trajectory points to be interpolated are interpolated.

[0171] Among them, during the interpolation process, the value of the mounting orientation is obtained through time linear interpolation, so that the motion state of the mounting changes more smoothly within the interpolation interval, thereby allowing the semi-trailer tractor to drive smoothly according to the final tractor motion trajectory.

[0172] Specifically, the time and direction of adjacent trajectory points are determined, the time point and position of the trajectory point to be interpolated are determined according to the time interval, and the θ value of the time point is calculated according to the linear ratio.

[0173] Step b4, after interpolating each segmented target motion trajectory, an optimized mounting motion trajectory is obtained.

[0174] The optimized mounting motion trajectory is formed by connecting the interpolated multiple target motion trajectories. The optimized mounting motion trajectory is more precise and accurate than the mounting motion trajectory obtained by the initial S200, which is convenient for controlling the semi-trailer tractor to tow the mounting to park at the target position and complete parking.

[0175] Based on the above first and second embodiments, a third embodiment of a parking path planning method is proposed.

[0176] Through the convex optimization algorithm, the optimal driving path can be determined according to the target position and target orientation, as well as the kinematic characteristics of the tractor and the mount, and the tractor's motion trajectory can be sent to the vehicle end in the form of a message frame, so that the semi-trailer tractor can drive based on the tractor's motion trajectory and park the mount at the target position while completing parking.

[0177] At the same time, sensor information can be used to monitor the position and status of the tractor and the mount in real time, and the trajectory can be adjusted and optimized to ensure that the mount can be accurately parked at the designated location. Specifically, it also involves judging whether the previous frame trajectory can be reused during the path planning process to improve computing efficiency. If the previous frame trajectory is still applicable in the current situation, there is no need to re-plan the complete trajectory, only to update it appropriately. This can reduce the consumption of computing resources and speed up the response speed of the vehicle system, which is especially important in scenarios with high real-time requirements.

[0178] Combination Figure 5As shown, in some embodiments, controlling the semi-trailer tractor to travel based on the motion trajectory of the tractor so that the mount completes parking based on the target state includes:

[0179] Step c1, during the process of controlling the semi-trailer tractor to travel according to the tractor motion trajectory, determining the minimum distance between the current position of the mount and the current segment of the mount motion trajectory based on a set detection frequency.

[0180] Based on the set detection frequency, the position and status of the tractor and the mount are monitored, and the distance between the current mount position and the nearest point on the trajectory is calculated. The trajectory can be the initial mount motion trajectory or the interpolated motion trajectory.

[0181] Step c2: when the minimum distance is less than a calibrated threshold, continue to control the semi-trailer tractor to travel based on the motion trajectory of the tractor, so that the mount completes parking based on the target state.

[0182] When the minimum distance between the current position of the mount and the nearest point of the current segment of the mount motion trajectory is less than the calibration threshold, it means that the current mount position is relatively close to the point on the trajectory. It can be considered that the trajectory of the previous frame is still applicable to the current situation to a certain extent and can be reused to update the trajectory to start from the vicinity of the current position. That is, continue to control the driving of the semi-trailer tractor based on the traction motion trajectory of the current segment published in the current frame, taking the point near the current position as the starting point.

[0183] Step c3, when the minimum distance is greater than or equal to the calibration threshold, taking the current position as the starting point, re-execute the step of obtaining the initial state of the mount when it is parked in the target coordinate system to update the mount motion trajectory.

[0184] When the minimum distance between the current position of the mount and the closest point of the current segment of the mount motion trajectory is greater than or equal to the calibration threshold, the current position is used as the initial position and the path of the semi-trailer tractor is replanned.

[0185] In this process, when the minimum distance is greater than or equal to the calibration threshold, it is determined whether the trajectory point corresponding to the minimum distance is the end point of the mount motion trajectory. If it is determined that the point closest to the trajectory of the current mount position is the end point of the trajectory, and the minimum distance is greater than or equal to the threshold, it is determined that the current trajectory execution is completed and the next trajectory is started, that is, the next trajectory is released, and the vehicle control system controls the vehicle to travel according to the next trajectory. Further, after determining that the point corresponding to the minimum distance is the end point and the distance is greater than or equal to the threshold, it is determined whether the speed of the current mount is reduced to the parameter that indicates that the vehicle is in a parking state. If so, it is determined to start the next trajectory.

[0186] After the next section of the mounted motion trajectory is released, it is also necessary to determine the point closest to the next section of the mounted motion trajectory based on the real-time position of the mount, and use this point in the vehicle motion trajectory as the starting point to control the semi-trailer tractor to travel.

[0187] If it is determined that the point closest to the trajectory of the current mount position is not the end point of the trajectory, and the minimum distance is greater than or equal to the threshold, it means that the actual driving situation deviates from the trajectory, and the mount motion trajectory of the previous frame cannot be reused and needs to be replanned. Therefore, the current position of the mount is used as the starting point, and the step of obtaining the initial state of the mount when it is parked in the target coordinate system is re-executed to update the mount motion trajectory.

[0188] In this embodiment, the above method is used to ensure that the trajectory is more consistent with the current actual situation, while reducing the number of optimization calculations during a parking process and reducing the workload of replanning the trajectory.

[0189] Based on the same application concept as the above method, the present application embodiment also proposes a parking path planning device, such as Figure 6 shown.

[0190] The device comprises:

[0191] An information acquisition module 602 is used to acquire an initial state of the mount when it is parked in a target coordinate system, wherein the target coordinate system is established with a target position in a target state when the mount is parked as an origin;

[0192] A path planning module 604 is used to solve the optimization objective function of the speed, acceleration and the articulation angle between the mount and the tractor in the target coordinate system based on the initial state, the target state and the preset constraint conditions to obtain the motion trajectory of the mount;

[0193] A coordinate conversion module 606 is used to convert the mounting motion trajectory into the world coordinate system where the tractor is located to obtain the tractor motion trajectory;

[0194] The parking control module 608 is used to control the semi-trailer tractor to travel based on the motion trajectory of the tractor so that the mount completes parking based on the target state.

[0195] The implementation process of the functions and effects of each module / submodule / unit in the above-mentioned device is specifically described in the implementation process of the corresponding steps in the above-mentioned method, and the same technical effect can be achieved, so it will not be repeated here.

[0196] Figure 7 An example of a physical structure diagram of a parking path planning device is shown in FIG. Figure 7As shown, the parking path planning device may include: a processor 810, a communication interface 820, a memory 830 and a communication bus 840, wherein the processor 810, the communication interface 820 and the memory 830 communicate with each other through the communication bus 840. The processor 810 may call the logic instructions in the memory 830 to execute the parking path planning method.

[0197] In addition, the logic instructions in the above-mentioned memory 830 can be implemented in the form of a software functional unit and can be stored in a computer-readable storage medium when it is sold or used as an independent product. Based on this understanding, the technical solution of the present application can be essentially or partly embodied in the form of a software product that contributes to the prior art, and the computer software product is stored in a storage medium, including several instructions to enable a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present application. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk, etc. Various media that can store program codes.

[0198] On the other hand, the present application also provides a computer program product, which includes a computer program. The computer program can be stored on a non-transitory computer-readable storage medium. When the computer program is executed by a processor, the computer can execute the parking path planning methods provided by the above methods.

[0199] On the other hand, the present application also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which is implemented when the computer program is executed by a processor to execute the parking path planning method provided by the above methods.

[0200] It should be noted that the technical solutions or technical features described in the above embodiments can be combined or supplemented with each other without causing conflicts. The scope of protection of this application is not limited to the precise structures described in the above embodiments and shown in the drawings; all modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of this application should be included in the scope of protection of this application.

Claims

1. A parking path planning method, characterized in that: Applied to a semi-trailer tractor, the semi-trailer tractor comprises a tractor and a mount, the method comprises: Acquire an initial state of the mount when it is parked in a target coordinate system, wherein the target coordinate system is established with a target position in a target state when the mount is parked as an origin; Based on the initial state, the target state and the preset constraint conditions, solving the optimization objective function about the speed, acceleration and the articulation angle between the mount and the tractor in the target coordinate system to obtain the mount motion trajectory; Convert the mounting motion trajectory into the world coordinate system where the tractor is located to obtain the tractor motion trajectory; The semi-trailer tractor is controlled to travel based on the motion trajectory of the tractor so that the load is parked based on the target state.

2. The parking path planning method according to claim 1, characterized in that: The obtaining of the initial state of the mount stop position in the target coordinate system includes: Obtaining the current state of the mount in the world coordinate system where the tractor is located and the target state to be achieved when parking; Establishing a target coordinate system with the target position in the target state as the coordinate origin; Based on the current state, an initial state of the mount in the target coordinate system is determined, wherein the initial state includes an initial position as a starting point of the planned motion trajectory of the mount.

3. The parking path planning method according to claim 1, characterized in that: The constraint conditions include kinematic constraints of the payload, and the kinematic constraints include constraints on velocity, acceleration, angular velocity, and an articulation angle between the tractor and the payload.

4. The parking path planning method according to claim 3, characterized in that: The constraint conditions also include position constraints and size constraints of the tractor and the mount; The position constraint is determined according to the initial state and is used to limit the driving range of the semi-trailer tractor.

5. The parking path planning method according to claim 1, characterized in that: The step of solving the optimization objective function of the speed, acceleration and the articulation angle between the mount and the tractor in the target coordinate system based on the initial state, the target state and the preset constraint conditions to obtain the mount motion trajectory includes: Call the solver; In the target coordinate system, based on the initial state, the target state and preset constraints, a convex optimization algorithm is used to solve the optimization objective function regarding speed, acceleration and the articulation angle between the mount and the tractor to obtain a mount motion trajectory.

6. The parking path planning method according to claim 1, characterized in that: After obtaining the mounting motion trajectory, the method further includes: According to the forward / backward changes of the mount during driving, the motion trajectory of the mount is segmented to obtain multiple segmented initial motion trajectories.

7. The parking path planning method according to claim 6, characterized in that: After the mounting motion trajectory is segmented according to the forward / reverse change of driving to obtain multiple segmented initial motion trajectories, the method further includes: The initial motion trajectory after multi-segment segmentation is interpolated to obtain the optimized mounting motion trajectory.

8. The parking path planning method according to claim 7, characterized in that: The initial motion trajectory includes a plurality of trajectory points, each of which corresponds to trajectory information including speed, direction and time. The interpolation of the initial motion trajectory after multiple segmentation to obtain the optimized mounting motion trajectory includes: The following steps are performed on the initial motion trajectory after two adjacent segments are divided: Determine the initial and final accelerations and the initial and final steering angular velocities of the two trajectory points according to the trajectory information of the two trajectory points in the initial and final states in the adjacent initial motion trajectory; According to the initial and final accelerations and the initial and final steering angular velocities, a trajectory point corresponding to the velocity zero point is determined as a target interpolation point; Based on the target interpolation point, re-segment the segmented initial motion trajectory to obtain a segmented target motion trajectory; Interpolate multiple segments of the target motion trajectory to obtain an optimized mounting motion trajectory.

9. The parking path planning method according to claim 8, characterized in that: The interpolating multiple segments of the target motion trajectory to obtain an optimized mounting motion trajectory includes: The following steps are performed for each segmented target motion trajectory: Determining the relative acceleration and relative steering angular velocity between adjacent trajectory points according to trajectory information of adjacent trajectory points in the current target motion trajectory; Based on the motion model of the constant relative acceleration and the constant relative steering angular velocity, the trajectory points to be interpolated are determined according to a preset interpolation interval method; Interpolating the current target motion trajectory based on the trajectory points to be interpolated; After interpolating each segmented target motion trajectory, an optimized mounting motion trajectory is obtained.

10. The parking path planning method according to claim 9, characterized in that: The preset interpolation interval mode includes time interval and distance interval.

11. The parking path planning method according to any one of claims 7 to 10, characterized in that: The step of converting the mounting motion trajectory into the world coordinate system where the tractor is located to obtain the tractor motion trajectory includes: The optimized mounting motion trajectory is converted into the world coordinate system where the tractor is located to obtain multiple segments of the tractor motion trajectory.

12. The parking path planning method according to claim 11, characterized in that: The controlling the semi-trailer tractor to travel based on the tractor motion trajectory so that the mount completes parking based on the target state includes: Control the semi-trailer tractor by combining each segment of the tractor motion trajectory with the gear information, so that the load is parked based on the target state; The gear information is associated with the forward / reverse change during the mounted driving, and is used to determine the driving direction of the semi-trailer tractor.

13. The parking path planning method according to claim 1, characterized in that: The step of converting the mounting motion trajectory into the world coordinate system where the tractor is located to obtain the tractor motion trajectory includes: Determining, based on the articulation angle between the mount and the tractor, a motion offset of the tractor relative to the mount in the target coordinate system; The mounting motion trajectory and the motion offset are converted into the world coordinate system where the tractor is located, and the position of each trajectory point in the mounting motion trajectory after conversion is added to the motion offset, and the obtained new trajectory points form the tractor motion trajectory.

14. The parking path planning method according to claim 1, characterized in that: Controlling the semi-trailer tractor to travel based on the motion trajectory of the tractor so that the mount completes parking based on the target state includes: During the driving process of the semi-trailer tractor controlled by the tractor motion trajectory, determining the minimum distance between the current position of the mount and the current segment of the mount motion trajectory based on a set detection frequency; When the minimum distance is less than a calibrated threshold, continue to control the semi-trailer tractor to travel based on the tractor motion trajectory, so that the mount completes parking based on the target state; When the minimum distance is greater than or equal to the calibration threshold, the step of obtaining the initial state of the mount when it is parked in the target coordinate system is re-executed with the current position as the starting point to update the mount motion trajectory.

15. The parking path planning method according to claim 14, characterized in that: The method further comprises: When the minimum distance is greater than or equal to the calibration threshold, determining whether the trajectory point corresponding to the minimum distance is the end point of the mounting motion trajectory; If not, taking the current position as the starting point, the step of obtaining the initial state of the mount when it is parked in the target coordinate system is re-executed to update the mount motion trajectory.

16. A parking path planning device, characterized in that: The method comprises a memory, a processor and a parking path planning program stored in the memory and executable on the processor, wherein the processor implements the steps of the parking path planning method according to any one of claims 1 to 15 when executing the parking path planning program.

17. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a parking path planning program, and when the parking path planning program is executed, the steps of the parking path planning method according to any one of claims 1 to 15 are implemented.

Images