Track optimization method and device, computer equipment and readable storage medium

Abstract

The invention relates to a trajectory optimization method and device, computer equipment and a readable storage medium. The method comprises the following steps: acquiring an initial trailer planning track of a trailer and an initial trailer planning track of a trailer connected with the trailer; calculating a reference trailer track point parameter of the trailer at the next time point based on the initial trailer track point parameter of the trailer at the current time point; calculating a reference trailer track point parameter of the trailer at the current time point based on the initial trailer track point parameter of the trailer at the current time point; calculating a trailer track point deviation between the initial trailer track point parameter and the reference trailer track point parameter, and calculating a trailer track point deviation between the initial trailer track point parameter and the reference trailer track point parameter; and optimizing the initial trailer planned trajectory based on the trailer trajectory point deviation and the trailer trajectory point deviation corresponding to each time point to obtain a target trailer planned trajectory of the trailer. By adopting the method, the accuracy of trajectory optimization can be improved.

Description

Technical Field

[0001] The present application relates to the field of computer technologies, and particularly to a trajectory optimization method, apparatus, computer device, and computer-readable storage medium. Background Art

[0002] With the development of automation and artificial intelligence technologies, robots have been widely applied to industrial production scenarios, such as factories: robots tow material vehicles through a towing and mounting device to transport materials, and achieve efficient transportation of materials through a path planning algorithm.

[0003] In the prior art, a trajectory planning method based on sampling and search is commonly used to plan the motion trajectory of a towing system. However, since the robot and the towed trailer are towed and connected through a towing and mounting device, the entire towing system has too many degrees of freedom and a large degree of motion flexibility. The trajectory planning method based on sampling and search is difficult to ensure the planning of a feasible and safe trajectory, nor can it ensure that the fitted trajectory satisfies the kinematic principle of the vehicle, resulting in a problem of low accuracy of the planned motion trajectory. Summary of the Invention

[0004] Based on this, it is necessary to provide a trajectory optimization method, apparatus, computer device, computer-readable storage medium, and computer program product capable of improving the accuracy of trajectory optimization for the above technical problems.

[0005] In a first aspect, the present application provides a trajectory optimization method applied to a towing system for a trailer towing a trailer, including:

[0006] Obtain an initial trailer planned trajectory of the trailer and an initial trailer planned trajectory of the trailer; the initial trailer planned trajectory includes initial trailer trajectory point parameters corresponding to multiple time points, and the initial trailer planned trajectory includes initial trailer trajectory point parameters corresponding to multiple time points;

[0007] Based on the initial trailer trajectory point parameters corresponding to the trailer at the current time point, calculate the reference trailer trajectory point parameters corresponding to the trailer at the next time point according to a preset kinematic model;

[0008] Based on the initial trailer trajectory point parameters corresponding to the trailer at the current time point, calculate the reference trailer trajectory point parameters corresponding to the trailer at the current time point according to a preset kinematic model;

[0009] Calculate the trailer trajectory point deviation between the initial trailer trajectory point parameters and the reference trailer trajectory point parameters corresponding to the next time point, and calculate the trailer trajectory point deviation between the initial trailer trajectory point parameters and the reference trailer trajectory point parameters corresponding to the current time point;

[0010] Based on the trailer trajectory point deviation and the towed vehicle trajectory point deviation corresponding to each time point respectively, optimize the initial towed vehicle planned trajectory to obtain the target towed vehicle planned trajectory of the towed vehicle.

[0011] In a second aspect, applied to a towing system for a towed vehicle to tow a trailer, the present application further provides a trajectory optimization device, including:

[0012] An initial trajectory acquisition module, configured to acquire the initial towed vehicle planned trajectory of the towed vehicle and the initial trailer planned trajectory of the trailer; the initial towed vehicle planned trajectory includes initial towed vehicle trajectory point parameters corresponding to multiple time points respectively, and the initial trailer planned trajectory includes initial trailer trajectory point parameters corresponding to multiple time points respectively;

[0013] A towed vehicle parameter calculation module, configured to calculate the reference towed vehicle trajectory point parameters corresponding to the next time point of the towed vehicle based on the initial towed vehicle trajectory point parameters corresponding to the towed vehicle at the current time point according to a preset kinematic model;

[0014] A trailer parameter calculation module, configured to calculate the reference trailer trajectory point parameters corresponding to the current time point of the trailer based on the initial towed vehicle trajectory point parameters corresponding to the towed vehicle at the current time point according to a preset kinematic model;

[0015] A trajectory deviation calculation module, configured to calculate the towed vehicle trajectory point deviation between the initial towed vehicle trajectory point parameters and the reference towed vehicle trajectory point parameters corresponding to the next time point respectively, and calculate the trailer trajectory point deviation between the initial trailer trajectory point parameters and the reference trailer trajectory point parameters corresponding to the current time point respectively;

[0016] A trajectory optimization module, configured to optimize the initial towed vehicle planned trajectory based on the trailer trajectory point deviation and the towed vehicle trajectory point deviation corresponding to each time point respectively to obtain the target towed vehicle planned trajectory of the towed vehicle.

[0017] In a third aspect, the present application further provides a computer device, including a memory and a processor, the memory stores a computer program, and when the processor executes the computer program, the steps of the above method are implemented.

[0018] In a fourth aspect, the present application further provides a computer-readable storage medium, on which a computer program is stored, and when the computer program is executed by a processor, the steps of the above method are implemented.

[0019] In a fifth aspect, the present application further provides a computer program product, including a computer program, and when the computer program is executed by a processor, the steps of the above method are implemented.

[0020] The above-mentioned trajectory optimization method, device, computer device, computer-readable storage medium, and computer program product obtain an initial trailer planned trajectory and an initial trailer planned trajectory, and according to the initial trailer trajectory point parameters at the current time point in the initial trailer planned trajectory, calculate the reference trailer trajectory point parameters of the trailer at the next time point according to a preset kinematic model, and calculate the reference trailer trajectory point parameters of the trailer at the current time point. Then, through the preset kinematic model, it is possible to obtain the reference trailer trajectory point parameters that reasonably exist for the trailer at the next time point during the movement of the trailer under the movement constraint conditions of the trailer, and obtain the reference trailer trajectory point parameters that reasonably exist for the trailer at the current time point during the movement of the trailer under the movement constraint conditions of the interaction between the trailer and the trailer, so as to obtain a reference basis for optimizing each trajectory point in the planned trajectories of the trailer and the trailer. Then, calculate the trailer trajectory point deviation between the initial trailer trajectory point parameters and the reference trailer trajectory point parameters corresponding to the next time point, and the trailer trajectory point deviation between the initial trailer trajectory point parameters and the reference trailer trajectory point parameters corresponding to the current time, ensuring the accuracy of the trailer trajectory point deviation and the trailer trajectory point deviation corresponding to each time point; by optimizing the initial trailer planned trajectory according to the trailer trajectory point deviation and the trailer trajectory point deviation, the accuracy of trajectory optimization is improved, and then the accuracy of the target trailer planned trajectory is improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] In order to more clearly illustrate the technical solutions in the embodiments of the present application or related technologies, the following will briefly introduce the drawings required for use in the description of the embodiments of the present application or related technologies. Obviously, the drawings in the following description are only some embodiments of the present application. For those of ordinary skill in the art, other related drawings can be obtained based on these drawings without creative efforts.

[0022] Figure 1 FIG. is a schematic diagram of the composition of a traction system to which the trajectory optimization method in an embodiment is applied;

[0023] Figure 2 FIG. is an application environment diagram of the trajectory optimization method in an embodiment;

[0024] Figure 3 FIG. is a schematic flowchart of the trajectory optimization method in an embodiment;

[0025] Figure 4 FIG. is a schematic diagram of the movement of a trailer in an embodiment;

[0026] Figure 5 FIG. is a schematic diagram of the connection between a trailer and a trailer in an embodiment;

[0027] Figure 6Schematic diagram of the steps of the trajectory optimization method in an embodiment;

[0028] Figure 7 Schematic diagram of the obstacle avoidance constraint of a towed vehicle in an embodiment;

[0029] Figure 8 Schematic diagram of the relative position of an obstacle in an embodiment;

[0030] Figure 9 Block diagram of the structure of the trajectory optimization device in an embodiment;

[0031] Figure 10 Internal structure diagram of a computer device in an embodiment;

[0032] Figure 11 Internal structure diagram of a computer device in another embodiment. Detailed implementation manners

[0033] In order to make the objectives, technical solutions and advantages of the present application clearer and more understandable, the present application will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application.

[0034] The trajectory optimization method provided by the embodiments of the present application can be applied to a traction system as shown in Figure 1 In this system, the trailer 110 towes the trailer 130 through the towing attachment device 120. The trailer 110 can be configured as a vehicle with the functions of sensing obstacle avoidance and autonomous navigation, such as an automated guided vehicle (AGV / AMR), a cleaning robot, a delivery robot, a reception robot, etc.; the trailer 130 can be configured as various transportation tools such as a rack, a trolley, a cart, a shelf, etc. with casters and a receiving space. The trailer 110 is traction-connected to the trailer 130 through a towing attachment device 120 such as a towing bar, a towing hook, a towing chain, a towing rope, etc. In the illustrated embodiment, a lidar 111 is provided at each of the right front corner and the left rear corner of the trailer 110, and the trailer 110 obtains radar point cloud data through the lidar 111 for environmental perception.

[0035] The trajectory optimization method provided by the embodiments of the present application can be applied to an application environment as shown in Figure 2 Among them, the terminal 202 communicates with the server 204 through the network. The data storage system can store the data that the server 204 needs to process. The data storage system can be integrated on the server 204, or can be placed on the cloud or other network servers.

[0036] In an exemplary embodiment, the terminal 202 obtains the initial trailer planning trajectory of the trailer and the initial trailer planning trajectory of the trailer; the initial trailer planning trajectory includes the initial trailer trajectory point parameters corresponding to multiple time points, and the initial trailer planning trajectory includes the initial trailer trajectory point parameters corresponding to multiple time points; the terminal 202 calculates the reference trailer trajectory point parameters corresponding to the trailer at the next time point according to a preset kinematic model based on the initial trailer trajectory point parameters corresponding to the trailer at the current time point; the terminal 202 calculates the reference trailer trajectory point parameters corresponding to the trailer at the current time point according to a preset kinematic model based on the initial trailer trajectory point parameters corresponding to the trailer at the current time point; the terminal 202 calculates the trailer trajectory point deviation between the initial trailer trajectory point parameters and the reference trailer trajectory point parameters corresponding to the next time point, and calculates the trailer trajectory point deviation between the initial trailer trajectory point parameters and the reference trailer trajectory point parameters corresponding to the current time point; the terminal 202 optimizes the initial trailer planning trajectory based on the trailer trajectory point deviations and trailer trajectory point deviations corresponding to each time point to obtain the target trailer planning trajectory of the trailer. The terminal 202 may store the target trailer planning trajectory of the trailer in the server 204. Among them, the terminal 202 may be Figure 1 the trailer shown. The server 204 may be an independent physical server, or a server cluster or distributed system composed of multiple physical servers, or a cloud server providing cloud computing services.

[0037] In an exemplary embodiment, as Figure 3 shown, a trajectory optimization method is provided. Taking the terminal in Figure 2 as an example, the method includes the following steps:

[0038] Step 302, obtaining the initial trailer planning trajectory of the trailer and the initial trailer planning trajectory of the trailer; the initial trailer planning trajectory includes the initial trailer trajectory point parameters corresponding to multiple time points, and the initial trailer planning trajectory includes the initial trailer trajectory point parameters corresponding to multiple time points.

[0039] Among them, the initial trailer planning trajectory refers to the motion trajectory to be optimized generated for the trailer. The initial trailer planning trajectory refers to the motion trajectory to be optimized generated for the trailer. The initial trailer trajectory point parameters refer to the motion parameters of the trailer at each trajectory point in the initial trailer planning trajectory, including parameters such as the position and speed of the trailer at this trajectory point. The initial trailer trajectory point parameters refer to the motion parameters of the trailer at each trajectory point in the initial trailer planning trajectory, including parameters such as the position and speed of the trailer at this trajectory point.

[0040] Exemplarily, the trailer is connected to the trailer through a towing and mounting device such as a towing hook. During the movement of the trailer, the trailer synchronously moves the trailer through the towing and mounting device. The terminal can pre-search for paths for the trailer and the trailer in the current environment respectively, which can be to search for paths in a local area of the current environment, such as to search for paths in an area with obstacles in the current environment, or to obtain the historical planned trajectories of the trailer and the trailer in the current environment respectively, to obtain the initial trailer planned trajectory of the trailer and the initial trailer planned trajectory of the trailer.

[0041] Step 304, based on the initial trailer trajectory point parameters corresponding to the trailer at the current time point, calculate the reference trailer trajectory point parameters corresponding to the trailer at the next time point according to a preset kinematic model.

[0042] Among them, the preset kinematic model refers to a mathematical model preset to describe the motion laws of the trailer, the trailer, and between the trailer and the trailer. The reference trailer trajectory point parameters refer to the trajectory point parameters of the trailer moving to the next time point under its corresponding motion laws calculated through the kinematic model based on the initial trailer trajectory point parameters at the current time point.

[0043] Exemplarily, a kinematic model for the trailer and the trailer is preset in the terminal to constrain the motion trajectories of the trailer and the trailer to satisfy the corresponding motion laws of the kinematic model. The preset kinematic model includes a motion model for the trailer, which is an operation function representing the trajectory point parameters of the trailer. Among them, the motion model of the trailer can include an optimization factor for optimizing the motion trend of the planned trajectory of the trailer, such as making the planned trajectory of the trailer smoother or rounder, etc.

[0044] After the terminal obtains the initial trailer planned trajectory, it obtains the initial trailer trajectory point parameters of the trailer at the current time point from the initial trailer planned trajectory, and calculates according to the motion model of the trailer based on the initial trailer trajectory point parameters to obtain the reference trailer trajectory point parameters of the trailer at the next time point. The reference trailer trajectory point parameters can be the position where the trailer should be when it moves to the next time point calculated according to the position and speed of the trailer at the current time point according to the motion model of the trailer.

[0045] In an exemplary embodiment, the optimization factor can be determined according to the environmental complexity of the current environment where the trailer is located, specifically, it can be the presence degree of obstacles in the current environment, the bending degree of the drivable area, etc. For example, when the trailer is in an area with more obstacles or more turns, the optimization factor can be set to a parameter that makes the motion trend of the trailer rounder (such as a parameter with a larger motion curvature), or when the trailer is in a relatively open or straight - driving area, the optimization factor can be set to a parameter that makes the motion trend of the trailer smoother (such as a parameter with a smaller motion curvature).

[0046] Step 306: Based on the initial trailer trajectory point parameters corresponding to the trailer at the current time point, calculate the reference trailer trajectory point parameters corresponding to the trailer at the current time point according to the preset kinematic model.

[0047] Among them, the reference trailer trajectory point parameters refer to the trailer trajectory point parameters of the trailer at the current time point calculated through the kinematic model according to the initial trailer trajectory point parameters at the current time point, which satisfy the motion law between the trailer and the trailer. It can be understood that the relative positions between the initial trailer trajectory point parameters and the reference trailer trajectory point parameters at the same time point satisfy the motion law.

[0048] Exemplarily, the preset kinematic model further includes a traction model between the trailer and the trailer, which represents the operation function of the trajectory point parameters of the trailer, and can be understood as being used to calculate the trajectory point parameters of the trailer following the trailer through the traction mounting device. The terminal calculates according to the initial trajectory point parameters of the trailer at the current time point according to the traction model of the trailer to obtain the reference trailer trajectory point parameters of the trailer at the current time point. The reference trailer trajectory point parameters can be the position and speed that the trailer should be at the same time point calculated according to the position and speed of the trailer at the current time point according to the traction model of the trailer.

[0049] Step 308: Calculate the trailer trajectory point deviation between the initial trailer trajectory point parameters and the reference trailer trajectory point parameters corresponding to the next time point respectively, and calculate the trailer trajectory point deviation between the initial trailer trajectory point parameters and the reference trailer trajectory point parameters corresponding to the current time point respectively;

[0050] Exemplarily, after the terminal obtains the reference trailer trajectory point parameters of the trailer at the next time point, it obtains the initial trailer trajectory point parameters of the trailer at the next time point from the initial trailer planned trajectory, and calculates the trailer trajectory point deviation between the initial trailer trajectory point parameters and the reference trailer trajectory point parameters at the next time point. This deviation represents the trajectory point deviation between the initial trailer trajectory point parameters of the trailer at the next time point and the reference trailer trajectory point parameters, such as position deviation, speed deviation, etc.

[0051] After the terminal obtains the reference trailer trajectory point parameters of the trailer at the current time point, it obtains the initial trailer trajectory point parameters of the trailer at the current time point from the initial trailer planned trajectory, and calculates the trailer trajectory point deviation between the initial trailer trajectory point parameters and the reference trailer trajectory point parameters at the current time point. This deviation represents the trajectory point deviation between the initial trailer trajectory point parameters of the trailer at the current time point and the reference trailer trajectory point parameters, such as position deviation, speed deviation, etc.

[0052] Step 310: Optimize the initial trailer planned trajectory based on the trailer trajectory point deviations and trailer trajectory point deviations corresponding to each time point respectively to obtain the target trailer planned trajectory of the trailer.

[0053] Exemplarily, for the trailer, the terminal calculates the reference trailer trajectory point parameters corresponding to the next time at each time point according to the initial trailer trajectory point parameters at each time point in the initial trailer planned trajectory, in accordance with the motion model of the trailer, to obtain the reference trailer trajectory point parameters of the trailer at each time point. According to the initial trailer trajectory point parameters and the reference trailer trajectory point parameters at each time point, the trailer trajectory point deviation at each time point is obtained. For the trailer hitch, the terminal calculates the reference trailer hitch trajectory point parameters corresponding to each time point according to the initial trailer trajectory point parameters at each time point in the initial trailer planned trajectory, in accordance with the towing model of the trailer hitch. According to the initial trajectory point parameters and the reference trailer trajectory point parameters at each time point in the initial trailer hitch planned trajectory, the trailer hitch trajectory point deviation at each time point is obtained.

[0054] The terminal optimizes the initial trailer planned trajectory according to the trailer trajectory point deviation and the trailer hitch trajectory point deviation, which may be to iteratively optimize the initial trailer planned trajectory using a preset optimizer until the iteration upper limit is met, to obtain the target trailer planned trajectory. The target trailer planned trajectory includes the positions and speeds (including linear speed and angular speed) of the optimized trailer at each time point, etc. Then, the terminal calculates the target trailer hitch planned trajectory of the trailer hitch according to the target trailer planned trajectory, in accordance with the towing model, so that the trailer moves according to the target trailer planned trajectory, and the trailer hitch moves according to the target trailer hitch planned trajectory.

[0055] Among them, the target trailer planned trajectory can be understood as a motion trajectory obtained by imposing motion constraints on the initial trailer planned trajectory (such as avoiding that the initial trailer planned trajectory may exceed the mechanical limits of the vehicle, for example, sharp turns causing skidding) and optimizing the smoothness of the trajectory (such as avoiding zigzag trajectories causing the vehicle to brake suddenly or turn sharply), on the basis of the obstacle avoidance path that complies with the initial trailer planned trajectory.

[0056] In the above trajectory optimization method, by obtaining the initial trailer planned trajectory and the initial trailer planned trajectory, according to the initial trailer trajectory point parameters at the current time point in the initial trailer planned trajectory, the reference trailer trajectory point parameters at the next time point of the trailer are calculated according to the preset kinematic model, and the reference trailer trajectory point parameters at the current time point of the trailer are calculated. Then, through the preset kinematic model, it is possible to obtain the reference trailer trajectory point parameters that reasonably exist at the next time point of the trailer during the movement of the trailer under the movement constraint conditions of the trailer, and obtain the reference trailer trajectory point parameters that reasonably exist at the current time point of the trailer during the movement of the trailer under the movement constraint conditions of the interaction between the trailer and the trailer, so as to obtain the reference basis for optimizing each trajectory point in the planned trajectories of the trailer and the trailer. Then, the trailer trajectory point deviation between the initial trailer trajectory point parameters and the reference trailer trajectory point parameters corresponding to the next time point is calculated, and the trailer trajectory point deviation between the initial trailer trajectory point parameters and the reference trailer trajectory point parameters corresponding to the current time is calculated, ensuring the accuracy of the trailer trajectory point deviation and the trailer trajectory point deviation corresponding to each time point; by optimizing the initial trailer planned trajectory according to the trailer trajectory point deviation and the trailer trajectory point deviation, the accuracy of the trajectory optimization is improved, and further the accuracy of the target trailer planned trajectory is improved.

[0057] In an exemplary embodiment, the initial trailer trajectory point parameters include the initial deflection angle, the initial position, the initial velocity, and the initial angular velocity of the trailer; step 304 calculates the reference trailer trajectory point parameters corresponding to the next time point of the trailer according to the preset kinematic model based on the initial trailer trajectory point parameters corresponding to the trailer at the current time point, including:

[0058] Based on the initial angular velocity of the trailer corresponding to the current time point, calculate the deflection amount of the trailer.

[0059] Based on the deflection amount, as well as the initial velocity, the initial deflection angle, and the initial position of the trailer corresponding to the current time point, calculate the reference position of the trailer corresponding to the next time point, and obtain the reference trailer trajectory point parameters corresponding to the next time point of the trailer.

[0060] Wherein, the deflection amount refers to the change amount of the deflection angle. The deflection angle refers to the angle at which the vehicle rotates around the vertical axis, representing the orientation of the vehicle in the horizontal plane. The initial position refers to the position where the trailer is located at the corresponding time point in the initial trailer trajectory point parameters. The initial velocity refers to the velocity of the trailer at the position where the trailer is located at the corresponding time point in the initial trailer trajectory point parameters. The initial angular velocity refers to the angular velocity of the trailer at the position where the trailer is located at the corresponding time point in the initial trailer trajectory point parameters. The reference position refers to the position where the trailer is located at the next time obtained through calculation.

[0061] Exemplarily, the initial trailer trajectory point parameters at each time point in the initial trailer planning trajectory include the initial deflection angle, initial position, initial speed, and initial angular velocity of the trailer. The terminal determines the interval duration from the current time point and the next time point in the initial trailer planning trajectory, and calculates the deflection angle corresponding to the next time point according to the initial angular velocity and the initial deflection angle. Then, the difference between the deflection angle at the next time point and the deflection angle at the current time point is calculated to obtain the deflection amount of the trailer.

[0062] The terminal obtains the optimization factor, and obtains the target deflection angle according to the optimization factor, the deflection amount, and the initial deflection angle at the current time point. According to the target deflection angle, the initial speed, and the initial position at the current time point, calculations are performed according to the motion model to obtain the reference position of the trailer corresponding to the next time point, and obtain the reference trailer trajectory point parameters corresponding to the next time of the trailer. The calculation of the reference trailer trajectory point parameters corresponding to the next time point (t + 1) is shown in formulas (1)-(4).

[0063]

[0064] Among them, and represent the reference position of the trailer at the next time point t + 1; and represent the initial position of the trailer at the current time point t; represents the initial deflection angle of the trailer at the current time point t; represents the deflection amount; is the target deflection angle; represents the initial speed of the trailer at the current time point t; represents the initial angular velocity of the trailer at the current time point t; represents the interval time between the current time point t and the next time t + 1; represents the deflection angle of the trailer at the next time point t + 1; a represents the optimization factor. Generally, a is 0.5. Further, in the case of setting , the movement of the trailer can be restricted from side shifting or drifting, so that the movement trajectory of the trailer is an arc with a certain turning radius. As shown in the schematic diagram of the trailer movement in Figure 4 , the trailer in Figure 4 is a differential mobile robot driven by two independently driven wheels. The trailer makes an arc movement around the center of the circle according to the deflection amount and moves from the position at time point t to the position at time point t + 1.

[0065] In this embodiment, by calculating the reference trailer trajectory point parameters of the trailer at the next time point, a reference object for optimizing the initial trailer trajectory point parameters of the trailer can be obtained. Therefore, when optimizing according to the trailer trajectory point deviation between the initial trailer trajectory point parameters and the reference trailer trajectory points, the optimization accuracy of the trailer trajectory point parameters is ensured.

[0066] In an exemplary embodiment, the initial trailer trajectory point parameters include the initial deflection angle, initial position, initial speed, and initial angular velocity of the trailer; the initial trailer hitch trajectory point parameters include the initial deflection angle of the trailer hitch. Step 306 calculates the reference trailer hitch trajectory point parameters of the trailer hitch at the current time point based on the initial trailer trajectory point parameters corresponding to the trailer at the current time point according to a preset kinematic model, including:

[0067] Obtain the connection parameters between the trailer and the trailer hitch.

[0068] Based on the initial deflection angle of the trailer hitch at the current time point and the initial deflection angle of the trailer at the current time point, calculate the relative deflection angle between the trailer and the trailer hitch at the current time point.

[0069] Based on the initial position, initial deflection angle of the trailer at the current time point, the initial deflection angle of the trailer hitch at the current time point, and the connection parameters, calculate the current position of the trailer hitch at the current time point.

[0070] Based on the initial speed, initial angular velocity, relative deflection angle, and connection parameters of the trailer at the current time point, calculate the current speed of the trailer hitch at the current time point.

[0071] Based on the current position and current speed of the trailer hitch at the current time point, obtain the reference trailer hitch trajectory point parameters of the trailer hitch at the time point.

[0072] Among them, the connection parameters refer to the parameters of the towing and mounting device used to connect the trailer and the trailer hitch. The relative deflection angle refers to the difference between the deflection angle of the trailer and the deflection angle of the trailer hitch.

[0073] Exemplarily, as Figure 5 shown in the connection schematic diagram of the trailer and the trailer hitch, the front vehicle is the trailer, the rear vehicle is the trailer hitch, and the trailer and the trailer hitch are connected by a towing and mounting device. Among them, the distance from the midpoint of the wheel connection line of the trailer to the tail of the trailer is Lh, the distance from the midpoint of the tail of the trailer to the midpoint of the wheel connection line of the trailer hitch is Lw, the body length of the trailer hitch is A, the width is B, the distance from the midpoint of the wheel connection line of the trailer hitch to the tail of the trailer hitch is C, the distance between the tail of the trailer and the head of the trailer hitch (i.e., the length of the towing and mounting device) is F, points E and D on the head of the trailer hitch are the positions where the reflective stickers are pasted, and the reflective sticker positions are D(x1, y1) and E(x2, y2), which are used to identify the trailer hitch. Among them, the straight line emitted from the midpoint of the wheel connection line of the trailer is the vector speed and vector angular velocity of the trailer at time point t and ), the arc emitted from the midpoint of the wheel connection line of the trailer is the planned trajectory of the trailer, including the trajectory point parameters of the trailer at each time point, such as the trajectory point parameters of the trailer at time point t and the trajectory point parameters of the next time point t + 1 . Among them, the deflection angle of the trailer at time point t and the deflection angle of the trailer at time point t + 1 can be calculated according to the speed and angular velocity of the trailer at the corresponding time point ( and ). The arc emitted from the midpoint of the wheel connection line of the semi-trailer is the planned trajectory of the semi-trailer, such as the trajectory point parameters of the semi-trailer at time point t and the trajectory point parameters of the next time point t + 1 . The deflection angle of the semi-trailer at time point t is , the deflection angle of the trailer at time point t and the deflection angle of the semi-trailer at time point t The included angle between them is the relative deflection angle at time point t , and the calculation of the relative deflection angle is shown in formula (5).

[0074]

[0075] Among them, buff is the limit parameter of the relative deflection angle.

[0076] The terminal can accurately calculate the deflection angle of the semi-trailer at time point t and the deflection angle of the next time point t + 1 as well as the length Lw from the midpoint of the trailer tail to the midpoint of the wheel connection line of the semi-trailer, as shown in formulas (6)-(10).

[0077]

[0078] Among them, represents the angular velocity of the semi-trailer at time point t; represents the body length of the trailer.

[0079] After the terminal calculates the relative deflection angle of the trailer and the semi-trailer at the current time point , and after connecting the parameters \(L_h\) and \(L_w\), according to the initial position, initial deflection angle of the trailer at the current time point, the initial deflection angle of the trailer at the current time point, and the connection parameters, calculate the current position of the trailer at the current time point according to the towing model, and calculate the current speed and current angular velocity of the trailer at the current time point according to the initial speed, initial angular velocity, relative deflection angle and connection parameters of the trailer at the current time point, and obtain the reference trailer trajectory point parameters of the trailer at the time point according to the current position, current speed and current angular velocity of the trailer at the current time point.

[0080] The calculation of the reference trailer trajectory point parameters is shown in formulas (11)-(14).

[0081]

[0082] Among them, and represent the current position of the trailer at time point \(t\); represent the current speed of the trailer at time point \(t\); represent the current angular velocity of the trailer at time point \(t\).

[0083] It can be understood that the preset kinematic model includes the above formulas (1)-(14). The terminal can calculate the reference trailer trajectory point parameters of the trailer at each time point and the reference trailer trajectory point parameters of the trailer at each time point through the preset kinematic model. Furthermore, when optimizing the initial trailer planned trajectory, corresponding motion constraints are imposed on the optimized trailer planned trajectory according to the above formulas (1)-(14).

[0084] In this embodiment, by calculating the reference trailer trajectory point parameters of the trailer at each time point, a reference object for optimizing the initial trailer trajectory point parameters of the trailer can be obtained, and by introducing the trailer trajectory point deviation of the trailer, the initial trailer planned trajectory of the trailer can be indirectly optimized when optimizing the initial trailer planned trajectory of the trailer, ensuring the accuracy of the planned trajectory of the trailer.

[0085] In an exemplary embodiment, step 308, based on the trailer trajectory point deviation and the trailer trajectory point deviation corresponding to each time point respectively, optimize the initial trailer planned trajectory to obtain the target trailer planned trajectory of the trailer, including:

[0086] Based on the trailer trajectory point deviation and the trailer trajectory point deviation corresponding to each time point respectively, optimize the initial trailer planned trajectory to obtain the intermediate trailer planned trajectory of the trailer, and use the intermediate trailer planned trajectory as the initial trailer planned trajectory of the trailer;

[0087] Based on the reference trailer trajectory point parameters corresponding to each time point, an intermediate trailer planned trajectory of the trailer is obtained, and the intermediate trailer planned trajectory is used as the initial trailer planned trajectory of the trailer;

[0088] Return to execute the step of calculating the reference trailer trajectory point parameters corresponding to the trailer at the next time point according to the preset kinematic model based on the initial trailer trajectory point parameters corresponding to the current time point until the convergence condition is reached, and obtain the target trailer planned trajectory of the trailer.

[0089] Among them, the intermediate trailer planned trajectory refers to the trailer planned trajectory that has been trajectory-optimized but not yet fully optimized. The intermediate trailer planned trajectory is a trailer planned trajectory obtained according to the reference trailer trajectory point parameters and the trajectory point parameters of each trailer calculated according to the traction model.

[0090] Exemplarily, the terminal optimizes the initial trailer planned trajectory using a preset optimizer according to the trailer trajectory point deviation and the trailer trajectory point deviation corresponding to each time point, and obtains the intermediate trailer planned trajectory. Then, according to the trailer trajectory point parameters of each time point in the intermediate trailer planned trajectory, the reference trailer trajectory point parameters of each time point are calculated according to the motion model of the trailer. When the trailer trajectory point deviation between the trailer trajectory point parameters in the intermediate trailer planned trajectory and the corresponding reference trailer trajectory point parameters is less than or equal to the preset threshold, the intermediate trailer planned trajectory is used as the target planned trajectory of the trailer.

[0091] When the trailer trajectory point deviation between the trailer trajectory point parameters in the intermediate trailer planned trajectory and the corresponding reference trailer trajectory point parameters is greater than the preset threshold, the intermediate trailer planned trajectory needs to be further optimized. The terminal calculates the reference trailer trajectory point parameters of the trailer at each time point according to the initial trailer trajectory point parameters of each time point in the initial trailer planned trajectory according to the traction model, obtains the intermediate trailer planned trajectory of the trailer according to the reference trailer trajectory point parameters of each time point, uses the intermediate trailer planned trajectory as the initial trailer planned trajectory of the trailer, and uses the intermediate trailer planned trajectory as the initial trailer planned trajectory of the trailer.

[0092] Then the terminal returns to execute the step of calculating the reference trailer trajectory point parameters corresponding to the trailer at the next time point according to the initial trailer trajectory point parameters corresponding to the current time point according to the preset kinematic model until the convergence condition is reached, such as both the trailer trajectory point deviation and the trailer trajectory point deviation are less than the preset threshold, or the number of iterations reaches the upper limit, etc., to obtain the target trailer planned trajectory of the trailer.

[0093] For example: The terminal calculates the reference trailer planning trajectory M1 of the trailer (including the reference trailer trajectory point parameters at each time point) according to the motion model for the initial trailer planning trajectory M0, and calculates the trailer trajectory point deviation (M1 - M0); the initial trailer planning trajectory is N0, and the intermediate trailer planning trajectory N1 of the trailer (including the reference trailer trajectory point parameters at each time point) is calculated according to the traction model for the initial trailer planning trajectory M0, and calculates the trailer trajectory point deviation (N1 - N0); the initial trailer planning trajectory M0 is optimized according to the trailer trajectory point deviation (M1 - M0) and the trailer trajectory point deviation (N1 - N0) to obtain the intermediate trailer planning trajectory M2.

[0094] The reference trailer planning trajectory M3 of the trailer is calculated according to the intermediate trailer planning trajectory M2, the trailer trajectory point deviation (M3 - M2) is calculated, the intermediate trailer planning trajectory N2 of the trailer is calculated according to the intermediate trailer planning trajectory M2, the trailer trajectory point deviation (N2 - N1) is calculated, and the intermediate trailer planning trajectory M2 is optimized according to the trailer trajectory point deviation (M3 - M2) and the trailer trajectory point deviation (N2 - N1) to obtain the optimized intermediate trailer planning trajectory M4, and so on, until the target trailer planning trajectory is obtained.

[0095] It can be understood that in the process of iterative optimization, for the trailer planning trajectory optimized in the previous (n - 1)th time, in the process of trajectory optimization in the current (n)th time, the required trajectory point deviations include: the trailer trajectory point deviation between the trajectory point parameters at each time point in the trailer planning trajectory optimized in the previous (n - 1)th time and the reference trajectory point parameters corresponding to the trajectory point parameters calculated according to the motion model, and the trailer trajectory point deviation between the trailer trajectory point parameters at each time point calculated according to the traction model for the trajectory point parameters at each time point in the trailer planning trajectory optimized in the previous (n - 2)th time (i.e., the trailer planning trajectory before the previous (n - 1)th optimization) and the trailer trajectory point parameters at each time point calculated according to the traction model for the trajectory point parameters at each time point in the trailer planning trajectory optimized in the previous (n - 1)th time. For example, the trailer trajectory point deviation (N2 - N1) between the trailer planning trajectory N1 calculated from the trailer planning trajectory M0 before optimization and the trailer planning trajectory N2 calculated from the trailer planning trajectory M2 after optimization is used to participate in the optimization of the trailer planning trajectory M2.

[0096] In an exemplary embodiment, in the process of separately optimizing the trajectories of the trailer and the trailer, it is also necessary to separately constrain the changes in the speed, acceleration, and jerk of the trailer and the trailer within a specific range, and the constraint conditions are shown in formula (15).

[0097] (15)

[0098] Wherein, represents the speed threshold of the trailer, represents the acceleration of the trailer, represents the acceleration threshold of the trailer, represents the jerk of the trailer, represents the jerk threshold of the trailer; represents the deflection angle of the trailer, represents the threshold of represents the acceleration of the deflection amount of the trailer, represents the threshold of represents the jerk of the deflection amount of the trailer, represents the threshold of

[0099] In this embodiment, during each iteration optimization process, based on the trailer trajectory point deviation calculated from the trailer planned trajectory optimized n - 1 times, and the trailer trajectory point deviation calculated from the trailer planned trajectories optimized n - 2 times and n - 1 times, the accuracy of the trajectory point deviation and the rationality of the iterative optimization can be ensured, thereby improving the optimization accuracy of the planned trajectory for the trailer.

[0100] In an exemplary embodiment, as Figure 6 shown, the trajectory optimization method further includes:

[0101] Step 602: Based on the body width of the trailer, expand the initial trailer planned trajectory to obtain a trailer planned trajectory area;

[0102] Step 604: Based on the body width of the trailer, expand the initial trailer planned trajectory to obtain a trailer planned trajectory area;

[0103] Step 606: Determine a target planned trajectory area from the trailer planned trajectory area and the trailer planned trajectory area;

[0104] Step 608: Based on the relative distance of the target obstacle relative to the regional edge of the target planned trajectory area, obtain an obstacle avoidance deviation;

[0105] Step 610: Based on the obstacle avoidance deviation, the trailer trajectory point deviation and the trailer trajectory point deviation corresponding to each time point, optimize the initial trailer planned trajectory to obtain the target trailer planned trajectory of the trailer.

[0106] Among them, the trailer planned trajectory area refers to the area occupied in the current environment according to the planned trajectory of the trailer with the body width of the trailer as the area width. The trailer planned trajectory area refers to the area occupied in the current environment according to the planned trajectory of the trailer with the body width of the trailer as the area width. The target planned trajectory area refers to the vehicle body area where the distance from the obstacle does not meet the obstacle avoidance condition, which can be the vehicle body area where the trailer and / or the trailer are located at a certain time point in the corresponding planned trajectory area. The obstacle avoidance deviation refers to the distance between the area edge of the target planned trajectory area and the obstacle.

[0107] Exemplarily, the initial trailer planned trajectory of the trailer is the motion trajectory obtained from the trajectory point parameters of the midpoint of the wheel connection line in the trailer at each time point. Similarly, the initial trailer planned trajectory of the trailer is the motion trajectory obtained from the trajectory point parameters of the midpoint of the wheel connection line in the trailer at each time point. After the terminal obtains the trailer trajectory point deviation and the trailer trajectory point deviation corresponding to each time point, according to the body width of the trailer, the initial trailer planned trajectory is inflated to obtain the trailer planned trajectory area, and according to the body width of the trailer, the initial trailer planned trajectory is inflated to obtain the trailer planned trajectory area.

[0108] The terminal obtains the current position of the target obstacle in the current environment. The target obstacle can be an obstacle to be avoided in the current environment based on the initial trailer planned trajectory, or an obstacle newly added in the current environment. The terminal calculates the first relative distance between the target obstacle and the trailer planned trajectory area, and the second relative distance between the target obstacle and the trailer planned trajectory area according to the current position of the target obstacle. Specifically, according to the current position of the target obstacle, the trailer area edge of the trailer planned trajectory area adjacent to the target obstacle is determined, and the trailer area edge of the trailer planned trajectory area adjacent to the target obstacle is determined, and then according to the current position of the target obstacle, the vertical distance between the target obstacle and the trailer area edge is calculated to obtain the first relative distance, and the vertical distance between the target obstacle and the trailer area edge is calculated to obtain the second relative distance. Based on the different positions of the target obstacle relative to the differential edge of the planned trajectory area, the first relative distance and the second relative distance can be positive or negative.

[0109] The terminal determines the relative position of the target obstacle with respect to the trailer planning trajectory area and the relative position of the target obstacle with respect to the trailer planning trajectory area according to whether the first relative distance and the second relative distance belong to positive or negative values. The relative position of the trailer includes the relative position where the target obstacle is inside the area of the trailer planning trajectory area (when the first relative distance is negative) or outside the area (when the first relative distance is positive), and the relative position of the trailer includes the relative position where the target obstacle is inside the area of the trailer planning trajectory area (when the second relative distance is negative) or outside the area (when the second relative distance is positive). The terminal determines the target planning trajectory area that collides with the target obstacle from the trailer planning trajectory area and the trailer planning trajectory area according to the relative position of the trailer, the relative position of the trailer, the first relative distance, and the second relative distance. For example, according to the relative position of the trailer, the relative position of the trailer, the first relative distance, and the second relative distance, from the trailer planning trajectory area and the trailer planning trajectory area, the trajectory planning area where the target obstacle is inside the area or the relative distance is less than the distance threshold is determined as the target planning trajectory area. The target planning trajectory area may include at least one of the trailer planning trajectory area and the trailer planning trajectory area.

[0110] Then the terminal obtains the relative distance of the target obstacle with respect to the area edge of the target planning trajectory area, that is, the vertical distance of the target obstacle with respect to the area edge of the target planning trajectory area. The obtained relative distance may be at least one of the first relative distance and the second relative distance. The terminal uses the obtained relative distance as the obstacle avoidance deviation, and then optimizes the initial trailer planning trajectory according to the obstacle avoidance deviation, the trailer trajectory point deviation and the trailer trajectory point deviation corresponding to each time point to obtain the target trailer planning trajectory of the trailer.

[0111] In an exemplary embodiment, as Figure 7 shown, a schematic diagram of the obstacle avoidance constraint of a towed vehicle is provided. Figure 7 In the front vehicle is the rectangular body of the trailer, the rear vehicle is the rectangular body of the trailer, and the irregular object is the target obstacle. The trailer moves to a certain trajectory point at a speed and an angular velocity , and when the trailer moves to a certain trajectory point , the first relative distance of the target obstacle with respect to the body edge of the trailer is d1, and the second relative distance of the target obstacle with respect to the body edge of the trailer is d2.

[0112] In this embodiment, during the process of optimizing the trailer planning trajectory, it is also necessary to perform obstacle avoidance constraints on the trailer planning trajectory and the trailer planning trajectory, which can ensure that the optimized planning trajectory effectively avoids obstacles and improves the accuracy of trajectory optimization.

[0113] In an exemplary embodiment, step 308, obtaining an obstacle avoidance deviation based on the relative distance of the target obstacle from the regional edge of the target planned trajectory area, includes:

[0114] When the target obstacle is located inside the target planned trajectory area, an obstacle avoidance deviation is obtained based on a first relative distance of the target obstacle from the regional edge of the trailer planned trajectory area and a second relative distance of the target obstacle from the regional edge of the trailer-mounted vehicle planned trajectory area;

[0115] When the target obstacle is located outside the target planned trajectory area, a target relative distance is determined from the first relative distance of the target obstacle from the regional edge of the trailer planned trajectory area and the second relative distance of the target obstacle from the regional edge of the trailer-mounted vehicle planned trajectory area; and an obstacle avoidance deviation is obtained based on the target relative distance.

[0116] Exemplarily, the terminal determines that the target obstacle is located inside the target planned trajectory area according to a first straight-line distance corresponding to the trailer planned trajectory area and the relative position of the trailer, and a second straight-line distance corresponding to the trailer-mounted vehicle planned trajectory area and the relative position of the trailer-mounted vehicle. The terminal uses the first relative distance of the target obstacle from the regional edge of the trailer planned trajectory area and the second relative distance of the target obstacle from the regional edge of the trailer-mounted vehicle planned trajectory area as the obstacle avoidance deviation. It can be understood that since the trajectory overlap between the trailer and the trailer-mounted vehicle is relatively high, when the target obstacle is located inside the target planned trajectory area in both the trailer planned trajectory area and the trailer-mounted vehicle planned trajectory area, collisions will occur with both the trailer and the trailer-mounted vehicle, so both the first relative distance and the second relative distance are used as the obstacle avoidance deviation.

[0117] When the terminal determines that the target obstacle is located outside the target planned trajectory area, it obtains the first relative distance of the target obstacle from the regional edge of the trailer planned trajectory area and the second relative distance of the target obstacle from the regional edge of the trailer-mounted vehicle planned trajectory area, determines the relative distance less than a preset distance threshold as the target relative distance, and uses the target relative distance as the obstacle avoidance deviation.

[0118] In an exemplary embodiment, as Figure 8 shown, a schematic diagram of the relative position of the obstacle is provided. The outer contour of the vehicle body of the trailer or the trailer-mounted vehicle is rectangular. The current position of the target obstacle is obtained. The current position can be the coordinate point OP of the target obstacle in the vehicle body coordinate system of the trailer or the trailer-mounted vehicle, and then it is determined whether the coordinate point OP is inside or outside the rectangular vehicle body. The coordinate point OP is outside the rectangular vehicle body as Figure 8 shown in -a. The point NP closest to the point OP is determined on the rectangular outer contour of the vehicle body, and the distance dis from the point OP to the point NP is calculated. At this time, the distance dis is a positive value. The coordinate point OP is inside the rectangular vehicle body asFigure 8 As shown in Figure -b, the point NP closest to the point OP is determined on the rectangular outer contour of the vehicle body, and the distance dis from the point OP to the point NP is calculated. At this time, the distance dis is negative. The obstacle avoidance constraints for the trailer and the semi - trailer are shown in Equation (16).

[0119] min(d1,d2) > min_obs_dis (16)

[0120] Where, min(d1,d2) is the minimum value of d1 and d2; min_obs_dis is the preset distance threshold.

[0121] In this embodiment, by judging whether the target obstacle is inside or outside the planned trajectory area and judging whether it is less than the preset distance threshold, the accuracy of the obstacle avoidance deviation can be guaranteed, and further the accuracy of the trajectory optimization can be guaranteed.

[0122] In an exemplary embodiment, the trajectory optimization method further includes:

[0123] Based on the target trailer trajectory point parameters at each time point in the target trailer planned trajectory, calculate the target semi - trailer trajectory point parameters at each time point according to the preset kinematic model;

[0124] Based on the target semi - trailer trajectory point parameters at each time point, obtain the target semi - trailer trajectory point parameters of the semi - trailer.

[0125] Exemplarily, after the terminal obtains the target trailer planned trajectory, according to the target trailer trajectory point parameters at each time point in the target trailer planned trajectory, calculate the target semi - trailer trajectory point parameters at each time point according to the towing model in the preset kinematic model. According to the target semi - trailer trajectory point parameters at each time point, obtain the target semi - trailer trajectory point parameters of the semi - trailer, which can ensure that the target semi - trailer planned trajectory is a motion trajectory that satisfies the motion constraints between the trailer and the semi - trailer. When the semi - trailer moves according to the target semi - trailer planned trajectory, the motion safety and stability of the semi - trailer can be guaranteed.

[0126] In an exemplary embodiment, the specific steps of the trajectory optimization are as follows:

[0127] Obtain the initial trailer planned trajectory of the trailer and the initial semi - trailer planned trajectory of the semi - trailer; the initial trailer planned trajectory includes the initial trailer trajectory point parameters corresponding to multiple time points, and the initial semi - trailer planned trajectory includes the initial semi - trailer trajectory point parameters corresponding to multiple time points; the initial trailer trajectory point parameters include the initial yaw angle, initial position, initial speed and initial angular velocity of the trailer; the initial semi - trailer trajectory point parameters include the initial yaw angle of the semi - trailer.

[0128] Calculate the deflection amount of the trailer based on the initial angular velocity corresponding to the trailer at the current time point; calculate the reference position corresponding to the trailer at the next time point based on the deflection amount, as well as the initial velocity, initial deflection angle, and initial position corresponding to the trailer at the current time point, to obtain the reference trailer trajectory point parameters corresponding to the trailer at the next time point.

[0129] Obtain the connection parameters between the trailer and the semi-trailer; calculate the relative deflection angle between the trailer and the semi-trailer at the current time point based on the initial deflection angle corresponding to the semi-trailer at the current time point and the initial deflection angle corresponding to the trailer at the current time point; calculate the current position of the semi-trailer at the current time point based on the initial position, initial deflection angle corresponding to the trailer at the current time point, the initial deflection angle corresponding to the semi-trailer at the current time point, and the connection parameters; calculate the current velocity of the semi-trailer at the current time point based on the initial velocity, initial angular velocity, relative deflection angle, and connection parameters corresponding to the trailer at the current time point; obtain the reference semi-trailer trajectory point parameters of the semi-trailer at the time point based on the current position and current velocity of the semi-trailer at the current time point.

[0130] Calculate the trailer trajectory point deviation between the initial trailer trajectory point parameters and the reference trailer trajectory point parameters corresponding to the next time point respectively, and calculate the semi-trailer trajectory point deviation between the initial semi-trailer trajectory point parameters and the reference semi-trailer trajectory point parameters corresponding to the current time point respectively.

[0131] Based on the body width of the trailer, expand the initial trailer planned trajectory to obtain the trailer planned trajectory area; based on the body width of the semi-trailer, expand the initial semi-trailer planned trajectory to obtain the semi-trailer planned trajectory area; determine the target planned trajectory area from the trailer planned trajectory area and the semi-trailer planned trajectory area; when the target obstacle is inside the area of the target planned trajectory area, obtain the obstacle avoidance deviation based on the first relative distance of the target obstacle relative to the area edge of the trailer planned trajectory area and the second relative distance of the target obstacle relative to the area edge of the semi-trailer planned trajectory area; or when the target obstacle is outside the area of the target planned trajectory area, determine the target relative distance from the first relative distance of the target obstacle relative to the area edge of the trailer planned trajectory area and the second relative distance of the target obstacle relative to the area edge of the semi-trailer planned trajectory area; obtain the obstacle avoidance deviation based on the target relative distance.

[0132] Based on the obstacle avoidance deviation, the trailer trajectory point deviation and the towed vehicle trajectory point deviation corresponding to each time point respectively, optimize the initial towed vehicle planned trajectory to obtain the intermediate towed vehicle planned trajectory of the towed vehicle, and use the intermediate towed vehicle planned trajectory as the initial towed vehicle planned trajectory of the towed vehicle; based on the reference trailer trajectory point parameters corresponding to each time point, obtain the intermediate trailer planned trajectory of the trailer, and use the intermediate trailer planned trajectory as the initial trailer planned trajectory of the trailer; return to execute the step of calculating the reference towed vehicle trajectory point parameters corresponding to the next time point of the towed vehicle according to the preset kinematic model based on the initial towed vehicle trajectory point parameters corresponding to the current time point until the convergence condition is reached, and obtain the target towed vehicle planned trajectory of the towed vehicle.

[0133] Based on the target towed vehicle trajectory point parameters at each time point in the target towed vehicle planned trajectory, calculate the target trailer trajectory point parameters of the trailer at each time point according to the preset kinematic model; based on the target trailer trajectory point parameters at each time point, obtain the target trailer trajectory point parameters of the trailer.

[0134] It should be understood that although the steps in the flowcharts involved in the above-described embodiments are sequentially shown according to the indication of the arrows, these steps are not necessarily executed in the order indicated by the arrows. Unless there is a clear indication in this article, the execution of these steps does not have a strict order limit, and these steps can be executed in other orders. Moreover, at least a part of the steps in the flowcharts involved in the above-described embodiments may include multiple steps or multiple stages. These steps or stages are not necessarily executed at the same moment, but can be executed at different moments. The execution order of these steps or stages is not necessarily sequential, but can be executed alternately or alternately with at least a part of other steps or steps or stages in other steps.

[0135] Based on the same inventive concept, an embodiment of the present application also provides a trajectory optimization device for implementing the above-mentioned trajectory optimization method. The implementation solution provided by this device to solve the problem is similar to the implementation solution described in the above method. Therefore, the specific limitations in one or more embodiments of the following trajectory optimization device can refer to the limitations on the trajectory optimization method in the above text, and will not be repeated here.

[0136] In an exemplary embodiment, as Figure 9 shown, a trajectory optimization device 900 is provided, which is applied to a towing system for towing a trailer by a towed vehicle, and includes: an initial trajectory acquisition module 902, a towed vehicle parameter calculation module 904, a trailer parameter calculation module 906, a trajectory deviation calculation module 908, and a trajectory optimization module 910, where:

[0137] The initial trajectory acquisition module 902 is configured to acquire the initial trailer planned trajectory of the trailer and the initial trailer planned trajectory of the trailer; the initial trailer planned trajectory includes initial trailer trajectory point parameters corresponding to multiple time points respectively, and the initial trailer planned trajectory includes initial trailer trajectory point parameters corresponding to multiple time points respectively;

[0138] The trailer parameter calculation module 904 is configured to calculate the reference trailer trajectory point parameters corresponding to the trailer at the next time point according to a preset kinematic model based on the initial trailer trajectory point parameters corresponding to the trailer at the current time point;

[0139] The trailer parameter calculation module 906 is configured to calculate the reference trailer trajectory point parameters corresponding to the trailer at the current time point according to a preset kinematic model based on the initial trailer trajectory point parameters corresponding to the trailer at the current time point;

[0140] The trajectory deviation calculation module 908 is configured to calculate the trailer trajectory point deviation between the initial trailer trajectory point parameters and the reference trailer trajectory point parameters corresponding to the next time point respectively, and calculate the trailer trajectory point deviation between the initial trailer trajectory point parameters and the reference trailer trajectory point parameters corresponding to the current time point respectively;

[0141] The trajectory optimization module 910 is configured to optimize the initial trailer planned trajectory based on the trailer trajectory point deviations and the trailer trajectory point deviations corresponding to each time point respectively, and obtain the target trailer planned trajectory of the trailer.

[0142] In an exemplary embodiment, the initial trailer trajectory point parameters include the initial deflection angle, the initial position, the initial speed, and the initial angular velocity of the trailer; the trailer parameter calculation module 904 is further configured to calculate the deflection amount of the trailer based on the initial angular velocity corresponding to the trailer at the current time point; based on the deflection amount, and the initial speed, the initial deflection angle, and the initial position corresponding to the trailer at the current time point, calculate the reference position of the trailer at the next time point, and obtain the reference trailer trajectory point parameters corresponding to the trailer at the next time point.

[0143] In an exemplary embodiment, the initial trailer trajectory point parameters include the initial deflection angle, initial position, initial speed, and initial angular velocity of the trailer; the initial trailer body trajectory point parameters include the initial deflection angle of the trailer body; the trailer body parameter calculation module 906 is further configured to obtain the connection parameters between the trailer and the trailer body; calculate the relative deflection angle between the trailer and the trailer body at the current time point based on the initial deflection angle of the trailer body corresponding to the current time point and the initial deflection angle of the trailer corresponding to the current time point; calculate the current position of the trailer body at the current time point based on the initial position, initial deflection angle of the trailer corresponding to the current time point, the initial deflection angle of the trailer body corresponding to the current time point, and the connection parameters; calculate the current speed of the trailer body at the current time point based on the initial speed, initial angular velocity, relative deflection angle, and connection parameters of the trailer corresponding to the current time point; and obtain the reference trailer body trajectory point parameters of the trailer body at the time point based on the current position and current speed of the trailer body at the current time point.

[0144] In an exemplary embodiment, the trajectory optimization module 910 is further configured to optimize the initial trailer planned trajectory based on the trailer body trajectory point deviation and the trailer trajectory point deviation corresponding to each time point to obtain the intermediate trailer planned trajectory of the trailer, and use the intermediate trailer planned trajectory as the initial trailer planned trajectory of the trailer; obtain the intermediate trailer body planned trajectory of the trailer body based on the reference trailer body trajectory point parameters corresponding to each time point, and use the intermediate trailer body planned trajectory as the initial trailer body planned trajectory of the trailer body; and return to execute the step of calculating the reference trailer trajectory point parameters corresponding to the next time point of the trailer according to the preset kinematic model based on the initial trailer trajectory point parameters corresponding to the current time point until the convergence condition is reached to obtain the target trailer planned trajectory of the trailer.

[0145] In an exemplary embodiment, the trajectory optimization device 900 is further configured to expand the initial trailer planned trajectory based on the body width of the trailer to obtain the trailer planned trajectory area; expand the initial trailer body planned trajectory based on the body width of the trailer body to obtain the trailer body planned trajectory area; determine the target planned trajectory area from the trailer planned trajectory area and the trailer body planned trajectory area; obtain the obstacle avoidance deviation based on the relative distance between the target obstacle and the area edge of the target planned trajectory area; and optimize the initial trailer planned trajectory based on the obstacle avoidance deviation, the trailer body trajectory point deviation and the trailer trajectory point deviation corresponding to each time point to obtain the target trailer planned trajectory of the trailer.

[0146] In an exemplary embodiment, the trajectory optimization device 900 is further configured to obtain an obstacle avoidance deviation based on a first relative distance of the target obstacle from the regional edge of the trailer planned trajectory region and a second relative distance of the target obstacle from the regional edge of the trailer planned trajectory region when the target obstacle is inside the target planned trajectory region; when the target obstacle is outside the target planned trajectory region, determine a target relative distance from the first relative distance of the target obstacle from the regional edge of the trailer planned trajectory region and the second relative distance of the target obstacle from the regional edge of the trailer planned trajectory region; and obtain the obstacle avoidance deviation based on the target relative distance.

[0147] In an exemplary embodiment, the trajectory optimization device 900 is further configured to calculate the target trailer trajectory point parameters of the trailer at each time point according to a preset kinematic model based on the target trailer trajectory point parameters at each time point in the target trailer planned trajectory; and obtain the target trailer trajectory point parameters of the trailer based on the target trailer trajectory point parameters at each time point.

[0148] Each module in the above trajectory optimization device can be implemented in whole or in part by software, hardware, and their combination. Each of the above modules can be embedded in or independent of a processor in a computer device in the form of hardware, or stored in a memory in the computer device in the form of software, so that the processor can call and execute the operations corresponding to each of the above modules.

[0149] In an exemplary embodiment, a computer device is provided. The computer device may be a server, and its internal structure diagram may be as Figure 10 shown. The computer device includes a processor, a memory, an input / output interface (Input / Output, abbreviated as I / O), and a communication interface. Among them, the processor, the memory, and the input / output interface are connected through a system bus, and the communication interface is connected to the system bus through the input / output interface. Among them, the processor of the computer device is used to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system, a computer program, and a database. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The database of the computer device is used to store data such as an initial trailer planned trajectory, an initial trailer planned trajectory, and a target trailer planned trajectory. The input / output interface of the computer device is used to exchange information between the processor and external devices. The communication interface of the computer device is used to communicate with an external terminal through a network connection. When the computer program is executed by the processor, a trajectory optimization method is implemented.

[0150] In an exemplary embodiment, a computer device is provided. The computer device may be a terminal, and its internal structure diagram may be asFigure 11 As shown in the figure. The computer device includes a processor, a memory, an input / output interface, a communication interface, a display unit, and an input device. Among them, the processor, the memory, and the input / output interface are connected through a system bus, and the communication interface, the display unit, and the input device are connected to the system bus through the input / output interface. Among them, the processor of the computer device is used to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system and computer programs. The internal memory provides an environment for the operation of the operating system and computer programs in the non-volatile storage medium. The input / output interface of the computer device is used to exchange information between the processor and external devices. The communication interface of the computer device is used to communicate with external terminals in a wired or wireless manner, and the wireless manner can be implemented through WIFI, a mobile cellular network, near field communication (NFC), or other technologies. When the computer program is executed by the processor, it implements a trajectory optimization method. The display unit of the computer device is used to form a visually visible picture, which can be a display screen, a projection device, or a virtual reality imaging device. The display screen can be a liquid crystal display screen or an electronic ink display screen. The input device of the computer device can be a touch layer covering the display screen, or a button, a trackball, or a touchpad provided on the computer device housing, or an external keyboard, touchpad, or mouse, etc.

[0151] Those skilled in the art can understand that Figure 10 - Figure 11 the structure shown in the figure is only a block diagram of some structures related to the solution of the present application, and does not constitute a limitation on the computer device to which the solution of the present application is applied. The specific computer device may include more or fewer components than those shown in the figure, or combine some components, or have different component arrangements.

[0152] In an exemplary embodiment, a computer device is provided, including a memory and a processor. A computer program is stored in the memory, and when the processor executes the computer program, the steps in the above method embodiments are implemented.

[0153] In one embodiment, a computer-readable storage medium is provided, on which a computer program is stored. When the computer program is executed by the processor, the steps in the above method embodiments are implemented.

[0154] In one embodiment, a computer program product is provided, including a computer program. When the computer program is executed by the processor, the steps in the above method embodiments are implemented.

[0155] It should be noted that the data involved in this application (including but not limited to the data for analysis, stored data, displayed data, etc.) are all data authorized by users or fully authorized by all parties, and the collection, use, and processing of relevant data need to comply with relevant regulations.

[0156] Those of ordinary skill in the art can understand that all or part of the processes in the methods of the above embodiments can be completed by instructing relevant hardware through a computer program. The computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above methods. Among them, any reference to a memory, database, or other medium used in the embodiments provided in this application can include at least one of non-volatile memory and volatile memory. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory, optical memory, high-density embedded non-volatile memory, resistive random access memory (ReRAM), magnetoresistive random access memory (MRAM), ferroelectric random access memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory can include random access memory (RAM) or external cache memory, etc. By way of illustration and not limitation, RAM can be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM), etc. The databases involved in the embodiments provided in this application can include at least one of relational databases and non-relational databases. Non-relational databases can include distributed databases based on blockchain, etc., and are not limited thereto. The processors involved in the embodiments provided in this application can be general-purpose processors, central processors, graphics processors, digital signal processors, programmable logic devices, data processing logics based on quantum computing, artificial intelligence (AI) processors, etc., and are not limited thereto.

[0157] The technical features of the above embodiments can be combined arbitrarily. For the sake of concise description, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, it should be considered to be within the scope recorded in this application.

[0158] The embodiments described above only represent several implementation manners of the present application. The description is relatively specific and detailed, but it should not be construed as a limitation on the patent scope of the present application. It should be noted that for those of ordinary skill in the art, without departing from the concept of the present application, several modifications and improvements can still be made, and these all belong to the protection scope of the present application. Therefore, the protection scope of the present application shall be subject to the appended claims.

Claims

1. A trajectory optimization method, applied to a traction system of a trailer towing a trailer, characterized in that: The method comprises: Acquire an initial trailer planning trajectory of the trailer and an initial trailer planning trajectory of the trailer; the initial trailer planning trajectory includes initial trailer trajectory point parameters corresponding to a plurality of time points respectively, and the initial trailer planning trajectory includes initial trailer trajectory point parameters corresponding to a plurality of time points respectively; Based on the initial trailer trajectory point parameters corresponding to the trailer at the current time point, calculating the reference trailer trajectory point parameters corresponding to the trailer at the next time point according to a preset kinematic model; Based on the initial trailer trajectory point parameters corresponding to the trailer at the current time point, calculating the reference trailer trajectory point parameters corresponding to the trailer at the current time point according to the preset kinematic model; Calculating the trailer trajectory point deviation between the initial trailer trajectory point parameters and the reference trailer trajectory point parameters corresponding to the next time point, and calculating the trailer trajectory point deviation between the initial trailer trajectory point parameters and the reference trailer trajectory point parameters corresponding to the current time point; Based on the trailer trajectory point deviations and the trailer trajectory point deviations corresponding to each time point, the initial trailer planning trajectory is optimized to obtain the target trailer planning trajectory of the trailer.

2. The method according to claim 1, characterized in that: The initial trailer trajectory point parameters include the initial deflection angle, initial position, initial velocity and initial angular velocity of the trailer; The calculating, based on the initial trailer trajectory point parameters corresponding to the trailer at the current time point, the reference trailer trajectory point parameters corresponding to the trailer at the next time point according to the preset kinematic model includes: Calculating the deflection amount of the trailer based on the initial angular velocity of the trailer at the current time point; Based on the deflection amount, and the initial speed, initial deflection angle and initial position of the trailer at the current time point, the reference position of the trailer at the next time point is calculated to obtain the reference trailer trajectory point parameters of the trailer at the next time point.

3. The method according to claim 1, characterized in that The initial trailer trajectory point parameters include the initial deflection angle, initial position, initial velocity and initial angular velocity of the trailer; the initial trailer trajectory point parameters include the initial deflection angle of the trailer; The calculating, based on the initial trailer trajectory point parameters corresponding to the trailer at the current time point, the reference trailer trajectory point parameters corresponding to the trailer at the current time point according to the preset kinematic model includes: Acquire connection parameters between the trailer and the tow vehicle; Calculate the relative deflection angle of the trailer and the trailer at the current time point based on the initial deflection angle of the trailer at the current time point and the initial deflection angle of the trailer at the current time point; Calculate the current position of the trailer at the current time point based on the initial position of the trailer at the current time point, the initial deflection angle, the initial deflection angle of the trailer at the current time point, and the connection parameter; Calculate the current speed of the trailer at the current time point based on the initial speed, initial angular speed, relative deflection angle and connection parameter of the trailer at the current time point; Based on the current position and current speed of the trailer at the current time point, a reference trailer trajectory point parameter of the trailer at the time point is obtained.

4. The method according to claim 1, characterized in that: The optimizing the initial trailer planning trajectory based on the trailer trajectory point deviations and the trailer trajectory point deviations corresponding to each time point to obtain the target trailer planning trajectory of the trailer includes: Based on the deviations of the trailer trajectory points and the trailer trajectory points corresponding to each time point, the initial trailer planning trajectory is optimized to obtain an intermediate trailer planning trajectory of the trailer, and the intermediate trailer planning trajectory is used as the initial trailer planning trajectory of the trailer; Based on the reference trailer trajectory point parameters corresponding to each time point, an intermediate trailer planning trajectory of the trailer is obtained, and the intermediate trailer planning trajectory is used as the initial trailer planning trajectory of the trailer; Return the initial trailer trajectory point parameters corresponding to the current time point, and execute the step of calculating the reference trailer trajectory point parameters corresponding to the trailer at the next time point according to the preset kinematic model until the convergence condition is reached to obtain the target trailer planning trajectory of the trailer.

5. The method according to claim 4, characterized in that The method further comprises: Based on the body width of the trailer, the initial trailer planning trajectory is expanded to obtain a trailer planning trajectory area; Based on the body width of the trailer, the initial trailer planning trajectory is expanded to obtain a trailer planning trajectory area; Determine a target planned trajectory area from the trailer planned trajectory area and the tow vehicle planned trajectory area; Obtaining an obstacle avoidance deviation based on a relative distance of a target obstacle to an edge of a region of the target planned trajectory region; Based on the obstacle avoidance deviation, the deviation of the trailer trajectory point and the deviation of the trailer trajectory point corresponding to each time point, the initial trailer planning trajectory is optimized to obtain the target trailer planning trajectory of the trailer.

6. The method according to claim 5, characterized in that Obtaining the obstacle avoidance deviation based on the relative distance of the target obstacle to the edge of the target planned trajectory area includes: When the target obstacle is located inside the target planned trajectory area, obtaining an obstacle avoidance deviation based on a first relative distance of the target obstacle to an edge of the trailer planned trajectory area and a second relative distance of the target obstacle to an edge of the trailer planned trajectory area; When the target obstacle is located outside the target planned trajectory area, determining a target relative distance from a first relative distance of the target obstacle to an area edge of the trailer planned trajectory area and a second relative distance of the target obstacle to an area edge of the trailer planned trajectory area; Obstacle avoidance deviation is obtained based on the target relative distance.

7. The method according to any one of claims 1 to 6, characterized in that: The method further comprises: Based on the target trailer trajectory point parameters at each time point in the planned trajectory of the target trailer, the target trailer trajectory point parameters at each time point are calculated according to the preset kinematic model; Based on the target trailer trajectory point parameters at each time point, the target trailer trajectory point parameters of the trailer are obtained.

8. A trajectory optimization device, applied to a traction system of a trailer towing a trailer, characterized in that: The device comprises: An initial trajectory acquisition module, used to acquire an initial trailer planning trajectory of the trailer and an initial trailer planning trajectory of the trailer; the initial trailer planning trajectory includes initial trailer trajectory point parameters corresponding to multiple time points respectively, and the initial trailer planning trajectory includes initial trailer trajectory point parameters corresponding to multiple time points respectively; A trailer parameter calculation module, configured to calculate reference trailer trajectory point parameters corresponding to the trailer at a next time point according to the preset kinematic model based on the initial trailer trajectory point parameters corresponding to the trailer at a current time point; The trailer parameter calculation module is used to calculate the reference trailer trajectory point parameters corresponding to the trailer at the current time point according to the preset kinematic model based on the initial trailer trajectory point parameters corresponding to the trailer at the current time point; A trajectory deviation calculation module, used to calculate the trailer trajectory point deviation between the initial trailer trajectory point parameters and the reference trailer trajectory point parameters corresponding to the next time point, and calculate the trailer trajectory point deviation between the initial trailer trajectory point parameters and the reference trailer trajectory point parameters corresponding to the current time point; The trajectory optimization module is used to optimize the initial trailer planning trajectory based on the trailer trajectory point deviations and the trailer trajectory point deviations corresponding to each time point, so as to obtain the target trailer planning trajectory of the trailer.

9. A computer device comprising a memory and a processor, wherein the memory stores a computer program, wherein: When the processor executes the computer program, the steps of the method according to any one of claims 1 to 7 are implemented.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 7 are implemented.

Images