Vehicle trajectory tracking method and device, medium and equipment

Abstract

The invention discloses a vehicle trajectory tracking method and device, a medium and equipment. The method comprises the following steps: determining an equipment reference track of the dragged equipment in a preset time period; determining a tractor planning track of the tractor in the preset time period according to the equipment reference track and a relative position relation between a tracking point and a connecting point of the towed equipment; determining a tractor control sequence required for reaching a first preset target in a simulation tracking mode; and the tractor control sequence is issued to a chassis control system of the tractor, so that the chassis control system carries out driving control on the tractor at the current moment according to the tractor control sequence, and trajectory tracking of the towed equipment at the current moment is achieved. According to the invention, the tracking effect of the dragged equipment can be improved, and the vehicle safety is improved.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving technology, and in particular to a vehicle trajectory tracking method, device, medium, and equipment. Background Art

[0002] The trajectory tracking module is an indispensable part of the autonomous driving system. It is used to output a certain control rate to control the vehicle to keep up with the planned trajectory of the tractor after the planning module completes the trajectory planning. For the autonomous driving system, the lateral trajectory and the longitudinal trajectory are often obtained through the decoupling planning scheme. In this way, the trajectory tracking is divided into lateral trajectory tracking and longitudinal trajectory tracking. Tracking the lateral trajectory can obtain the control rate of the front wheel angle, and tracking the longitudinal trajectory can obtain the control rate of the longitudinal acceleration. The control rate of the front wheel angle and the control rate of the longitudinal acceleration are sent to the vehicle controller response module, so that the vehicle can keep up with the planned trajectory of the tractor.

[0003] Common lateral and longitudinal trajectory tracking algorithm modeling is designed to make the rear axle of the vehicle keep up with the planned trajectory of the tractor. However, for a vehicle including a tractor and a towed equipment, the effect of common trajectory tracking algorithms is not very good, because the modeling method of common trajectory tracking algorithms is: take the tractor as the search starting point, search for a tractor planned trajectory for the tractor, and then make the tractor keep up with the tractor planned trajectory. When the tractor moves forward, the towed equipment may cut inward, and when it moves backward, it may cut outward. In severe cases, it may cause the bucket to fold. Therefore, the actual tracking effect of the towed equipment cannot be guaranteed, and the vehicle safety is poor. For example, the towed equipment collides with surrounding obstacles. Summary of the invention

[0004] In order to solve at least one of the above technical problems, the present invention provides a vehicle trajectory tracking method and device, medium, and equipment.

[0005] According to a first aspect, a vehicle trajectory tracking method provided by an embodiment of the present invention is applied to a vehicle including a tractor and a towed device, the method comprising:

[0006] Determine a device reference trajectory of the towed device within a preset time period; wherein the initial moment of the preset time period is the current moment;

[0007] Determine the planned trajectory of the tractor within the preset time period according to the equipment reference trajectory and the relative position relationship between the tracking point and the connection point of the towed equipment; wherein the connection point is the connection point between the towed equipment and the tractor;

[0008] Determine the tractor control sequence required to achieve the first preset goal by means of simulation tracking; wherein the tractor control sequence is a sequence formed by each tractor control quantity corresponding to each moment within the preset time period; the first preset goal is that the tractor can track the planned trajectory of the tractor;

[0009] The tractor control sequence is sent to the chassis control system of the tractor, so that the chassis control system controls the driving of the tractor according to the tractor control sequence at the current moment, thereby realizing the trajectory tracking of the towed equipment at the current moment.

[0010] In one embodiment, the device reference trajectory is a first device reference trajectory; correspondingly, determining the planned trajectory of the tractor within the preset time period according to the device reference trajectory and the relative position relationship between the tracking point and the connection point of the towed device includes:

[0011] Determine the device state sequence required to achieve the second preset goal by means of simulation tracking; wherein the device state sequence is a sequence formed by the state quantities of the towed device corresponding to each moment in the preset time period, and the second preset goal is that the towed device can track the reference trajectory of the first device;

[0012] Determining a corresponding second device reference trajectory according to the device state sequence;

[0013] According to the reference trajectory of the second device and the relative position relationship between the tracking point and the connection point of the towed device, the connection point planning trajectory corresponding to the connection point is determined, and the connection point planning trajectory is used as the tractor planning trajectory.

[0014] In one embodiment, determining the device state sequence required to achieve the second preset goal by means of simulation tracking includes:

[0015] S11, obtaining the towed device state quantity of the towed device at the current moment;

[0016] S12, determining the control amount of the towed device required to achieve the second preset target at the current moment according to the state amount of the towed device at the current moment;

[0017] S13, according to the state quantity of the towed device and the control quantity of the towed device at the current moment, and based on the bicycle kinematics model, simulate and track the next moment to obtain the state quantity of the towed device at the next moment; and use the next moment as the initial first moment;

[0018] S14, determining whether the current first moment is the end moment of the preset time period;

[0019] S15, if the current first moment is not the termination moment, determining the control amount of the towed device required to achieve the second preset target at the first moment according to the state amount of the towed device at the first moment; performing simulation tracking on the moment next to the first moment according to the state amount of the towed device and the control amount of the towed device at the first moment and based on the bicycle kinematics model, obtaining the state amount of the towed device at the moment next to the first moment; taking the moment next to the first moment as the first moment, and returning to S14;

[0020] S16: If the current first moment is the termination moment, the state quantities of the towed equipment corresponding to each moment in the preset time period are formed into the equipment state sequence.

[0021] In one embodiment, the towed device control amount includes the towed device front wheel angle and the towed device speed; correspondingly, the towed device control amount required at the current moment to achieve the second preset target according to the towed device state amount at the current moment includes:

[0022] Determine the front wheel turning angle of the towed device required to achieve the second preset target at the current moment by using a pure tracking algorithm and the state quantity of the towed device at the current moment;

[0023] The speed of the towed equipment required at the current moment to achieve the second preset target is determined through a proportional-integral-differential control algorithm.

[0024] In one embodiment, the state quantity of the towed device includes the position coordinates of the towed device and the heading angle of the towed device; correspondingly, the front wheel turning angle of the towed device required to achieve the second preset target at the current moment determined by the pure tracking algorithm and the state quantity of the towed device at the current moment includes:

[0025] Determine a pre-anchor point according to the position coordinates of the towed device and the reference trajectory of the first device; wherein the pre-anchor point is a point on the reference trajectory of the first device that is a preset distance away from the tracking point of the towed device;

[0026] Determine, according to the heading angle of the towed device, an angle between a line connecting the pre-anchor point and the tracking point of the towed device and a body of the towed device;

[0027] The front wheel turning angle of the towed equipment at the current moment is calculated according to the included angle.

[0028] In one embodiment, the calculating the front wheel turning angle of the towed equipment at the current moment according to the included angle includes:

[0029] The first calculation formula is used to calculate the front wheel turning angle of the towed equipment, and the first calculation formula is:

[0030]

[0031] Among them, δ f is the front wheel turning angle of the towed equipment, L is the wheelbase between the front and rear axles of the towed equipment, and α is the angle; l d is the preset distance.

[0032] In one embodiment, the determining of the tractor control sequence required to achieve the first preset target by means of simulation tracking includes:

[0033] S21, obtaining the tractor state quantity of the tractor at the current moment;

[0034] S22, determining the tractor control amount required to achieve the first preset target at the current moment according to the tractor state amount at the current moment;

[0035] S23, according to the tractor state quantity and the tractor control quantity at the current moment, and based on the bicycle kinematics model, simulate and track the next moment to obtain the tractor state quantity at the next moment; and use the next moment as the initial second moment;

[0036] S24, determining whether the current second moment is the end moment of the preset time period;

[0037] S25, if the current second moment is not the termination moment, determining the tractor control amount required to achieve the first preset target at the second moment according to the tractor state at the second moment; performing simulation tracking on the moment next to the second moment based on the bicycle kinematics model according to the tractor state and the tractor control at the second moment, to obtain the tractor state at the moment next to the second moment; taking the moment next to the second moment as the second moment, and returning to S24;

[0038] S26. If the current second moment is the termination moment, the tractor control quantities corresponding to each moment in the preset time period are formed into the tractor control sequence.

[0039] According to a second aspect, a vehicle trajectory tracking device provided by an embodiment of the present invention is applied to a vehicle including a tractor and a towed device, and the device includes:

[0040] A first determination module is used to determine the device reference trajectory of the towed device within a preset time period; wherein the initial moment of the preset time period is the current moment;

[0041] A second determination module is used to determine the planned trajectory of the tractor within the preset time period according to the equipment reference trajectory and the relative position relationship between the tracking point and the connection point of the towed equipment; wherein the connection point is the connection point between the towed equipment and the tractor;

[0042] A first simulation module is used to determine the tractor control sequence required to achieve a first preset goal by means of simulation tracking; wherein the tractor control sequence is a sequence formed by each tractor control quantity corresponding to each moment within the preset time period; and the first preset goal is that the tractor can track the planned trajectory of the tractor;

[0043] The control sending module is used to send the tractor control sequence to the chassis control system of the tractor, so that the chassis control system controls the driving of the tractor at the current moment according to the tractor control sequence, thereby realizing the trajectory tracking of the towed equipment at the current moment.

[0044] According to the third aspect, an embodiment of the present invention provides a computer-readable storage medium, on which a computer program is stored. When the computer program is executed in a computer, the computer is caused to execute the method provided in the first aspect.

[0045] According to a fourth aspect, a computing device provided by an embodiment of the present invention includes a memory and a processor, wherein the memory stores executable code, and when the processor executes the executable code, the method provided by the first aspect is implemented.

[0046] The embodiment of the present invention has the following technical effects: firstly, a device reference trajectory is planned for the towed device, then the planned trajectory of the tractor is derived based on the device reference trajectory, and then the tractor control sequence required for the tractor to track the planned trajectory of the tractor is determined by simulation tracking, and then the tractor control sequence is sent to the chassis control system, so that the chassis control system will control the tractor according to the tractor control sequence, so that the tractor can track the planned trajectory of the tractor, thereby realizing the trajectory tracking of the towed device. It can be seen that the embodiment of the present invention transforms the trajectory tracking problem of the towed device into the tracking problem of the tractor for another trajectory through problem transformation, simplifies the problem, and thus occupies a small amount of calculation, while also ensuring the tracking effect of the towed device, thereby reducing the safety hazards of the towed device during the towing process and improving the safety of the vehicle. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] In order to more clearly illustrate the specific implementation methods of the present invention or the technical solutions in the prior art, the drawings required for use in the specific implementation methods or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are some implementation methods of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0048] Figure 1 is a flow chart of a vehicle trajectory tracking method in one embodiment of the present invention;

[0049] Figure 2 is a schematic diagram of various parameters used in the derivation process of the front wheel turning angle of the towed equipment in one embodiment of the present invention;

[0050] Figure 3 is a schematic diagram of a first device reference trajectory, a second device reference trajectory, and a tractor planned trajectory in one embodiment of the present invention;

[0051] Figure 4 It is a structural block diagram of a vehicle trajectory tracking device in one embodiment of the present invention. DETAILED DESCRIPTION

[0052] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution of the present invention will be described clearly and completely below. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work belong to the scope of protection of the present invention.

[0053] In a first aspect, an embodiment of the present invention provides a vehicle trajectory tracking method, which is applied to a vehicle including a tractor and a towed device, see Figure 1 , the method comprises the following steps S110 to S140:

[0054] S110, determining a device reference trajectory of the towed device within a preset time period; wherein the initial moment of the preset time period is the current moment;

[0055] The vehicle includes a tractor and a towed device. The towed device may be a trailer. The trailer may also be called a trailer. It has no power system and can only move under the drive of the tractor. In this case, the vehicle may be a full-trailer vehicle or a semi-trailer vehicle. Of course, the towed device is not limited to a trailer. For example, it may also be an airplane.

[0056] In step S110, a search algorithm may be used to search for a device reference trajectory within a preset time period for the towed device. The search algorithm may be a hybrid A* algorithm or an A* algorithm. Both the hybrid A* algorithm and the A* algorithm are graph search algorithms, and the hybrid A* algorithm is an improvement on the A* algorithm. The difference from the A* algorithm is that the reference path searched by the hybrid A* algorithm takes into account the kinematic constraints of the towed device, that is, satisfies the maximum curvature constraint of the towed device, so that the maximum curvature using the hybrid A* algorithm will not exceed the maximum curvature constraint, which conforms to the kinematic law.

[0057] For example, taking the towed equipment as the search starting point, or further taking the center of the rear axle of the trailer as the search starting point, a search algorithm is used to search for a reference trajectory for the towed equipment, and the searched reference trajectory is feasible, for example, the reference trajectory does not intersect with obstacles, that is, if the towed equipment runs according to the reference trajectory, it will not collide with obstacles.

[0058] In actual scenarios, the method provided by the embodiment of the present invention can be applied to parking scenarios. The device reference trajectory obtained by the above search can be a trajectory from the current position of the towed device to the parking position; of course, it can also be a short trajectory starting from the current position of the towed device, which does not reach the parking position. During the driving process of the vehicle, the method provided by the embodiment of the present invention is executed once every period of time to continuously search for and track the reference trajectory, and finally accurately reach the parking position. Of course, it is not limited to parking scenarios, but also used in driving scenarios.

[0059] S120, determining the planned trajectory of the tractor within the preset time period according to the equipment reference trajectory and the relative position relationship between the tracking point and the connection point of the towed equipment; wherein the connection point is the connection point between the towed equipment and the tractor;

[0060] For example, if the towed equipment is a trailer, the tracking point is the center of the rear axle of the trailer.

[0061] For example, if the tractor and the towed equipment are connected in an articulated manner, then the above-mentioned connection point is the articulated point.

[0062] If the hybrid A* algorithm is used in S110, the searched equipment reference trajectory is in accordance with the kinematic law. In the case where the equipment reference trajectory is in accordance with the kinematic law, the tractor planning trajectory can be directly derived from the equipment reference trajectory. The specific process is as follows: since the equipment reference trajectory is actually the reference trajectory of the tracking point of the towed equipment, based on the relative position relationship between the tracking point of the towed equipment and the connection point, the connection point planning trajectory mentioned below can be derived, and the connection point is also located on the tractor, so the connection point planning trajectory can be used as the tractor planning trajectory.

[0063] If other search algorithms are used in S110, the device reference trajectory obtained by searching may not conform to the kinematic law. The solution to this situation will be described in S121 to S123 below.

[0064] S130, determining a tractor control sequence required to achieve a first preset goal by means of simulation tracking; wherein the tractor control sequence is a sequence formed by various tractor control quantities corresponding to various moments within the preset time period; and the first preset goal is that the tractor can track the planned trajectory of the tractor;

[0065] It is understandable that the simulation tracking method refers to making the tractor track the planned trajectory of the tractor by means of simulation, that is, realizing simulation tracking rather than real tracking.

[0066] Among them, the tractor control quantity, for example, the tractor front wheel angle, the tractor speed.

[0067] S140: Send the tractor control sequence to the chassis control system of the tractor, so that the chassis control system controls the tractor to travel according to the tractor control sequence at the current moment, thereby tracking the trajectory of the towed equipment at the current moment.

[0068] It can be seen that in S130, the tractor control sequence can be determined by simulation tracking, and in S140, the tractor performs real driving control according to the tractor control sequence, which will realize the tractor tracking the planned trajectory of the tractor in the actual scene. When the tractor tracks the planned trajectory of the tractor, the trajectory tracking of the towed equipment is realized.

[0069] That is to say, in order to realize the task of tracking the trajectory of the towed equipment, it is necessary to realize the tracking of the planned trajectory of the tractor by the tractor. In order to realize the tracking of the planned trajectory of the tractor by the tractor, it is necessary to determine the tractor control sequence, which includes the tractor control quantities corresponding to each moment. After determining the tractor control sequence, the tractor control sequence is sent to the tractor chassis control system, and the tractor chassis control system can control the driving of the tractor according to the tractor control quantities in the tractor control sequence, so as to realize the tractor tracking the planned trajectory of the tractor.

[0070] In actual scenarios, the method provided by the embodiment of the present invention can be executed once at each moment, that is, at each moment, the above S110-S140 is repeatedly executed as the current moment. For example, the above S110-S140 is executed once every 1 minute, and the above preset time period can be set to 5 minutes. In this way, the tractor control sequence corresponding to the next 5 minutes at each moment can be determined through the above steps, and when the chassis control system performs control according to the tractor control sequence, it only needs the tractor control amount at the first moment in the tractor control sequence, that is, the tractor control amount corresponding to the current moment. The reason why the tractor control sequence corresponding to the next 5 minutes needs to be provided here is that there may be a problem of obvious data jump between the tractor control amount corresponding to the current moment and the tractor control amount corresponding to the next moment, and the tractor control amount at the next moment and the moments thereafter are all changing smoothly. At this time, the tractor control amount corresponding to the current moment can be corrected, or driving control is not performed. If there is no obvious data jump between the tractor control amount corresponding to the current moment and the tractor control amount corresponding to the next moment, the tractor control amount corresponding to the current moment is used for driving control. It can be seen that since there may be errors in the calculation process of the tractor control quantity, this method can reduce the control problems caused by the incorrect tractor control quantity.

[0071] In one embodiment, the device reference trajectory is a first device reference trajectory; correspondingly, determining the planned trajectory of the tractor within the preset time period according to the device reference trajectory and the relative position relationship between the tracking point and the connection point of the towed device in S120 may include S121 to S123:

[0072] S121, determining a device state sequence required to achieve a second preset goal by means of simulation tracking; wherein the device state sequence is a sequence formed by state quantities of the towed device corresponding to each moment in the preset time period, and the second preset goal is that the towed device can track the reference trajectory of the first device;

[0073] It is understandable that simulation tracking refers to enabling the towed device to track the reference trajectory of the first device by means of simulation, i.e., achieving simulation tracking rather than real tracking. The purpose of simulation tracking in S121 is to determine the device state sequence required to ensure that the towed device can track the reference trajectory of the first device.

[0074] The state quantity of the towed device may include the position coordinates of the towed device and the heading angle of the towed device, and the position coordinates are the horizontal and vertical coordinates.

[0075] S122, determining a corresponding second device reference trajectory according to the device state sequence;

[0076] Since the device state sequence includes the state quantities of the towed device corresponding to each moment in the preset time period, and the towed device state quantities include position coordinates, a discrete point can be determined according to a position coordinate, and multiple discrete points can be determined according to multiple position coordinates in the device state sequence, and these discrete points are connected to obtain a second device reference trajectory.

[0077] S123. Determine a connection point planning trajectory corresponding to the connection point according to the reference trajectory of the second device and the relative position relationship between the tracking point and the connection point of the towed device, and use the connection point planning trajectory as the tractor planning trajectory.

[0078] The above S121 to S123 are a solution adopted when the first device reference trajectory searched by the search algorithm may not conform to the kinematic law. The general process is mainly as follows: on the basis of the first device reference trajectory, the corresponding second device reference trajectory is determined by simulation tracking. Since the second device reference trajectory has been simulated and tracked, it conforms to the kinematic law. Therefore, the connection point planning trajectory can be derived from the second device reference trajectory according to the relative position relationship between the tracking point and the connection point of the towed device. Since the connection point is also located on the tractor, the connection point planning trajectory can be used as the tractor planning trajectory.

[0079] Of course, if the distance between the rear axle center of the tractor and the connection point is relatively far, the planned trajectory of the rear axle center of the tractor can also be derived from the planned trajectory of the connection point to serve as the planned trajectory of the tractor.

[0080] See also Figure 3 , trajectory a is the reference trajectory of the first device, trajectory b is the reference trajectory of the second device, trajectory c is the planned trajectory of the tractor, k1 is the towed device, and k2 is the tractor.

[0081] Furthermore, the device state sequence required to achieve the second preset target by means of simulation tracking in the above S121 may include S11 to S16:

[0082] S11, obtaining the towed device state quantity of the towed device at the current moment;

[0083] S12, determining the control amount of the towed device required to achieve the second preset target at the current moment according to the state amount of the towed device at the current moment;

[0084] S13, according to the state quantity of the towed device and the control quantity of the towed device at the current moment, and based on the bicycle kinematics model, simulate and track the next moment to obtain the state quantity of the towed device at the next moment; and use the next moment as the initial first moment;

[0085] S14, determining whether the current first moment is the end moment of the preset time period;

[0086] S15, if the current first moment is not the termination moment, determining the control amount of the towed device required to achieve the second preset target at the first moment according to the state amount of the towed device at the first moment; performing simulation tracking on the moment next to the first moment according to the state amount of the towed device and the control amount of the towed device at the first moment and based on the bicycle kinematics model, obtaining the state amount of the towed device at the moment next to the first moment; taking the moment next to the first moment as the first moment, and returning to S14;

[0087] S16: If the current first moment is the termination moment, the state quantities of the towed equipment corresponding to each moment in the preset time period are formed into the equipment state sequence.

[0088] For example, the position coordinates and heading angle of the towed device at the current moment t0 are obtained through the positioning perception system. According to the state quantity of the towed device corresponding to the current moment t0, the control quantities such as the front wheel turning angle of the towed device and the speed of the towed device corresponding to the current moment t0 are determined. According to the state quantity of the towed device and the control quantity of the towed device corresponding to the current moment t0, forward simulation is performed based on the bicycle kinematics model to obtain the state quantity of the towed device corresponding to the next moment t1. According to the state quantity of the towed device corresponding to the next moment t1, the control quantity of the towed device corresponding to the next moment t1 is determined. According to the state quantity of the towed device corresponding to the next moment t1 and the control quantity of the towed device, forward simulation is performed based on the bicycle kinematics model to obtain the state quantity of the towed device corresponding to the next moment t2 of the next moment t1. This is repeated until the state quantity of the towed device at the last moment in the preset time period is obtained. The state quantities of the towed device corresponding to each moment in the preset time period form a device state sequence.

[0089] In one embodiment, when the towed device control amount includes the towed device front wheel angle and the towed device speed, the towed device control amount required to achieve the second preset target at the current moment according to the towed device state amount at the current moment in S12 may include S12-1 to S12-2:

[0090] S12-1, determining the front wheel turning angle of the towed device required to achieve the second preset target at the current moment by using a pure tracking algorithm and the state quantity of the towed device at the current moment;

[0091] Among them, the pure tracking algorithm is a commonly used algorithm in autonomous driving control systems and is often used for lateral trajectory tracking.

[0092] S12-2. Determine the speed of the towed equipment required to achieve the second preset target at the current moment through a proportional-integral-differential control algorithm.

[0093] Among them, the proportional-integral-differential control algorithm is the PID algorithm. The PID algorithm is a commonly used algorithm in closed-loop control systems. PID is the abbreviation of Proportion, Integral, and Differential. In a closed-loop control system, a feedback loop is introduced, and the deviation between the actual speed value and the expected speed value is used to control the entire system, thereby outputting an actual speed value that is gradually close to the expected speed value, and the actual speed value is used as the speed of the towed device in the above-mentioned towed device control quantity. The expected speed value is the target speed value of the towed device specified in advance.

[0094] Among them, the PID algorithm can be used for longitudinal trajectory tracking in the autonomous driving control system.

[0095] It can be seen that the control amount of the towed device at each moment can be determined through S12-1 to S12-2, and assuming that the chassis control system controls the driving of the towed device according to the control amount of the towed device, the towed device will keep up with the first device reference trajectory.

[0096] Furthermore, in the case where the state quantity of the towed device includes the position coordinates of the towed device and the heading angle of the towed device, the step of determining the front wheel turning angle of the towed device required to achieve the second preset target at the current moment by using the pure tracking algorithm and the state quantity of the towed device at the current moment in S12-1 may include the following steps (1) to (3):

[0097] (1) determining a pre-anchor point according to the position coordinates of the towed device and the reference trajectory of the first device; wherein the pre-anchor point is a point on the reference trajectory of the first device that is a preset distance away from the tracking point of the towed device;

[0098] (2) determining the angle between the line connecting the pre-anchor point and the tracking point of the towed device and the body of the towed device according to the heading angle of the towed device;

[0099] (3) Calculating the front wheel turning angle of the towed equipment at the current moment according to the included angle.

[0100] Specifically, in (3), a first calculation formula may be used to calculate the front wheel turning angle of the towed device, and the first calculation formula is:

[0101]

[0102] Among them, δ f is the front wheel turning angle of the towed equipment, L is the wheelbase between the front and rear axles of the towed equipment, and α is the angle; l d is the preset distance.

[0103] It can be seen that the above steps (1) to (3) are actually the process of determining the front wheel angle of the towed equipment by a pure tracking algorithm.

[0104] For example, see Figure 2 , select the preset distance l from the towed device on the device reference trajectory path d The point is used as the pre-anchor point C, and the tracking point A of the towed equipment can reach the pre-anchor point C by driving at a certain turning radius R, that is, fixed circle motion. Figure 2 The X / Y in the figure represents the earth coordinate system, 0 is the origin of the earth coordinate system, and x / y represents the coordinate system of the towed device with the tracking point of the towed device as the origin. is the heading angle, α is the angle between the preview point C and the towed device itself, L is the wheelbase between the front and rear axles of the towed device, (x r ,y r ) is the coordinate of the preview point C, (x h ,y h ) is the coordinate of the rear axle center of the towed device, i.e., tracking point A. B is the front axle center of the towing device, which can also be understood as the position of the front wheel, and O is the origin of the arc from A to C.

[0105] based on Figure 2 , the front wheel turning angle δ of the towed equipment f The derivation process of the first calculation formula is roughly as follows:

[0106] First, because ΔAOC is an isosceles triangle, in ΔAOB, AB is perpendicular to OA, so ∠AOC is equal to:

[0107] Then, in order for the tracking point of the towed device to track the arc to reach point C, the sine theorem relationship that ΔAOC needs to satisfy is:

[0108]

[0109] Then, we can deduce from the above sine theorem relationship:

[0110]

[0111] The front wheel turning angle δ f Need to satisfy the relationship:

[0112]

[0113] Finally, bring R into this relationship and get the first calculation formula:

[0114]

[0115] In one embodiment, the tractor control sequence required to achieve the first preset target determined by simulation tracking in S130 may include S21 to S26:

[0116] S21, obtaining the tractor state quantity of the tractor at the current moment;

[0117] S22, determining the tractor control amount required to achieve the first preset target at the current moment according to the tractor state amount at the current moment;

[0118] S23, according to the tractor state quantity and the tractor control quantity at the current moment, and based on the bicycle kinematics model, simulate and track the next moment to obtain the tractor state quantity at the next moment; and use the next moment as the initial second moment;

[0119] S24, determining whether the current second moment is the end moment of the preset time period;

[0120] S25, if the current second moment is not the termination moment, determining the tractor control amount required to achieve the first preset target at the second moment according to the tractor state at the second moment; performing simulation tracking on the moment next to the second moment based on the bicycle kinematics model according to the tractor state and the tractor control at the second moment, to obtain the tractor state at the moment next to the second moment; taking the moment next to the second moment as the second moment, and returning to S24;

[0121] S26. If the current second moment is the termination moment, the tractor control quantities corresponding to each moment in the preset time period are formed into the tractor control sequence.

[0122] It can be seen that S21-S26 are similar to S11-S16 in the above text, except that in S11-S16 what needs to be determined is the device state sequence corresponding to the towed device, and in S21-S26 what needs to be determined is the tractor control sequence corresponding to the tractor.

[0123] It is understandable that under the existing tracking problem, the towed device needs to track the reference trajectory. It is not feasible to directly use the existing pure tracking algorithm, PID algorithm, etc., because the search starting point of the existing control algorithm is the rear axle center of the tractor, and the control quantity is calculated based on the bicycle kinematic model. There is a difference between the bicycle kinematic model and the follow-up model of the towed device. It is foreseeable that the direct use effect will not be very good. For this reason, the embodiment of the present invention converts the problem: assuming that there is an imaginary trajectory, as long as the tractor keeps up with this imaginary trajectory, it can drive the towed device to keep up with the reference trajectory. Then the original problem of the towed device tracking the reference trajectory becomes the problem of the tractor tracking the imaginary trajectory. What needs to be done now is to find this imaginary trajectory. The so-called imaginary trajectory is the planned trajectory of the tractor. In this way, the embodiment of the present invention can still use pure tracking algorithms, PID algorithms, etc.

[0124] It can be seen that in order to solve the problem of tracking effect of the towed equipment in the prior art, in the embodiment of the present invention, a device reference trajectory is first planned for the towed equipment, and then the planned trajectory of the tractor is derived based on the device reference trajectory, and then the tractor control sequence required for the tractor to track the planned trajectory of the tractor is determined by simulation tracking, and then the tractor control sequence is sent to the chassis control system, so that the chassis control system will control the tractor according to the tractor control sequence, so that the tractor can track the planned trajectory of the tractor, thereby realizing the trajectory tracking of the towed equipment. It can be seen that the embodiment of the present invention transforms the trajectory tracking problem of the towed equipment into the tracking problem of the tractor for another trajectory through problem transformation, simplifies the problem, and thus occupies a small amount of calculation, and also ensures the tracking effect of the towed equipment, thereby reducing the safety hazards of the towed equipment during the towing process and improving the safety of the vehicle. Moreover, the trajectory tracking method provided by the embodiment of the present invention can be used as a hot start algorithm used by the optimizer to provide a high-quality initial solution for the optimization problem and improve the convergence speed of the optimizer.

[0125] In summary, the embodiment of the present invention provides a reverse or serial approach. The towed device is regarded as an imaginary vehicle, and a corresponding device reference trajectory is directly searched for the towed device. Then, the planned trajectory of the towing vehicle is inferred from the device reference trajectory. The embodiment of the present invention is a new approach to solving the trajectory tracking problem of the towed device. Without introducing redundant calculations, the trajectory tracking problem of the towed device is processed as a serial tracking problem, thereby ensuring the tracking effect of the towed device and avoiding safety hazards.

[0126] In a second aspect, an embodiment of the present invention provides a vehicle trajectory tracking device, which is applied to a vehicle including a tractor and a towed device, see Figure 4 , the device 100 comprises:

[0127] A first determination module 110 is used to determine a device reference trajectory of the towed device within a preset time period; wherein the initial moment of the preset time period is the current moment;

[0128] A second determination module 120 is used to determine the planned trajectory of the tractor within the preset time period according to the device reference trajectory and the relative position relationship between the tracking point and the connection point of the towed device; wherein the connection point is the connection point between the towed device and the tractor;

[0129] The first simulation module 130 is used to determine the tractor control sequence required to achieve the first preset goal by means of simulation tracking; wherein the tractor control sequence is a sequence formed by each tractor control quantity corresponding to each moment within the preset time period; the first preset goal is that the tractor can track the planned trajectory of the tractor;

[0130] The control sending module 140 is used to send the tractor control sequence to the chassis control system of the tractor, so that the chassis control system controls the driving of the tractor according to the tractor control sequence at the current moment, thereby realizing the trajectory tracking of the towed equipment at the current moment.

[0131] In one embodiment, the device reference trajectory is a first device reference trajectory; correspondingly, the second determination module includes:

[0132] A first simulation submodule is used to determine the device state sequence required to achieve a second preset goal by means of simulation tracking; wherein the device state sequence is a sequence formed by the state quantities of the towed device corresponding to each moment in the preset time period, and the second preset goal is that the towed device can track the reference trajectory of the first device;

[0133] A first determination submodule, configured to determine a corresponding second device reference trajectory according to the device state sequence;

[0134] The second determination submodule is used to determine the connection point planning trajectory corresponding to the connection point according to the reference trajectory of the second device and the relative position relationship between the tracking point and the connection point of the towed device, and use the connection point planning trajectory as the tractor planning trajectory.

[0135] In one embodiment, the first simulation submodule specifically includes:

[0136] A first acquiring unit, configured to execute S11, and acquire a state quantity of the towed device at the current moment;

[0137] A first determining unit is used to execute S12, determining a control amount of the towed device required to achieve the second preset target at the current moment according to the state amount of the towed device at the current moment;

[0138] A first simulation unit is used to execute S13, perform simulation tracking at the next moment based on the state quantity of the towed device and the control quantity of the towed device at the current moment and the bicycle kinematic model to obtain the state quantity of the towed device at the next moment; and use the next moment as the initial first moment;

[0139] A first judging unit, configured to execute S14, judging whether the current first moment is the end moment of the preset time period;

[0140] a second determining unit, configured to execute S15, if the current first moment is not the termination moment, determine the control amount of the towed device required to achieve the second preset target at the first moment according to the state amount of the towed device at the first moment; perform simulation tracking on the next moment after the first moment according to the state amount of the towed device and the control amount of the towed device at the first moment and based on a bicycle kinematics model, to obtain the state amount of the towed device at the next moment after the first moment; take the next moment after the first moment as the first moment, and return to the first judging subunit to execute S14;

[0141] The sequence forming unit is used to execute S16, if the current first moment is the termination moment, forming the device state sequence with the state quantities of the towed device corresponding to each moment in the preset time period.

[0142] In one embodiment, the control amount of the towed device includes the front wheel turning angle of the towed device and the speed of the towed device; correspondingly, the first determining unit includes:

[0143] A turning angle determination subunit, configured to determine the turning angle of the front wheels of the towed device required at the current moment to achieve the second preset target by using a pure tracking algorithm and the state quantity of the towed device at the current moment;

[0144] The speed determination subunit is used to determine the speed of the towed equipment required at the current moment to achieve the second preset target through a proportional-integral-differential control algorithm.

[0145] In one embodiment, the state quantity of the towed device includes the position coordinates of the towed device and the heading angle of the towed device; correspondingly, the turning angle determination subunit is specifically used to: determine a pre-anchor point according to the position coordinates of the towed device and the first device reference trajectory; wherein the pre-anchor point is a point on the first device reference trajectory that is a preset distance away from the tracking point of the towed device; determine an angle between a line connecting the pre-anchor point and the tracking point of the towed device and a body of the towed device according to the heading angle of the towed device; and calculate the front wheel turning angle of the towed device at the current moment according to the angle.

[0146] In one embodiment, the turning angle determination subunit is specifically used to calculate the turning angle of the front wheel of the towed device using a first calculation formula, wherein the first calculation formula is:

[0147]

[0148] Among them, δf is the front wheel turning angle of the towed equipment, L is the wheelbase between the front and rear axles of the towed equipment, and α is the angle; l d is the preset distance.

[0149] In one embodiment, the first simulation module is used to perform the following steps:

[0150] S21, obtaining the tractor state quantity of the tractor at the current moment;

[0151] S22, determining the tractor control amount required to achieve the first preset target at the current moment according to the tractor state amount at the current moment;

[0152] S23, according to the tractor state quantity and the tractor control quantity at the current moment, and based on the bicycle kinematics model, simulate and track the next moment to obtain the tractor state quantity at the next moment; and use the next moment as the initial second moment;

[0153] S24, determining whether the current second moment is the end moment of the preset time period;

[0154] S25, if the current second moment is not the termination moment, determining the tractor control amount required to achieve the first preset target at the second moment according to the tractor state at the second moment; performing simulation tracking on the moment next to the second moment based on the bicycle kinematics model according to the tractor state and the tractor control at the second moment, to obtain the tractor state at the moment next to the second moment; taking the moment next to the second moment as the second moment, and returning to S24;

[0155] S26. If the current second moment is the termination moment, the tractor control quantities corresponding to each moment in the preset time period are formed into the tractor control sequence.

[0156] It is understandable that the explanation, specific implementation, beneficial effects, examples, etc. of the relevant contents in the device provided in the embodiment of the present invention can be found in the corresponding parts of the method provided in the first aspect, and will not be repeated here.

[0157] In a third aspect, an embodiment of the present invention provides a computer-readable medium, on which computer instructions are stored. When the computer instructions are executed by a processor, the processor executes the method provided in the first aspect.

[0158] Specifically, a system or device equipped with a storage medium can be provided, on which software program code that implements the functions of any of the above-mentioned embodiments is stored, and a computer (or CPU or MPU) of the system or device can be enabled to read and execute the program code stored in the storage medium.

[0159] In this case, the program code itself read from the storage medium can realize the function of any one of the above-mentioned embodiments, and thus the program code and the storage medium storing the program code constitute a part of the present invention.

[0160] The storage medium embodiments for providing the program code include a floppy disk, a hard disk, a magneto-optical disk, an optical disk (such as CD-ROM, CD-R, CD-RW, DVD-ROM, DVD-RAM, DVD-RW, DVD+RW), a magnetic tape, a non-volatile memory card, and a ROM. Alternatively, the program code can be downloaded from a server computer by a communication network.

[0161] In addition, it should be clear that the functions of any of the above embodiments can be implemented not only by executing the program code read by the computer, but also by enabling an operating system operating on the computer to complete part or all of the actual operations based on instructions from the program code.

[0162] In addition, it can be understood that the program code read from the storage medium is written to a memory provided in an expansion board inserted into the computer or to a memory provided in an expansion module connected to the computer, and then based on the instructions of the program code, a CPU installed on the expansion board or expansion module is enabled to perform part or all of the actual operations, thereby realizing the functions of any of the above-mentioned embodiments.

[0163] It is understandable that the explanation, specific implementation methods, beneficial effects, examples, etc. of the relevant contents in the computer-readable medium provided in the embodiment of the present invention can be found in the corresponding parts of the method provided in the first aspect, and will not be repeated here.

[0164] In a fourth aspect, an embodiment of the present specification provides a computing device, including a memory and a processor, wherein the memory stores executable code, and when the processor executes the executable code, it implements the method in any embodiment of the specification.

[0165] It is understandable that the explanation, specific implementation, beneficial effects, examples, etc. of the relevant contents in the computing device provided in the embodiment of the present invention can be found in the corresponding parts of the method provided in the first aspect, and will not be repeated here.

[0166] It should be noted that the terms used in the present invention are only for describing specific embodiments, rather than limiting the scope of the present application. As shown in the present specification and claims, unless the context clearly indicates an exception, the words "one", "a", "a kind of" and / or "the" do not specifically refer to the singular, but may also include the plural. The terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such a process, method or device. In the absence of further restrictions, the elements defined by the sentence "include one..." do not exclude the presence of other identical elements in the process, method or device including the elements.

[0167] It should also be noted that the orientations or relative positional relationships indicated by the terms "center", "up", "down", "left", "right", "vertical", "horizontal", "inside", "outside", etc. are based on the orientations or relative positional relationships shown in the drawings, and are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as a limitation on the present invention. Unless otherwise clearly specified and limited, the terms "installed", "connected", "connected", etc. should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be a direct connection, or it can be an indirect connection through an intermediate medium, or it can be a connection between the two elements. For those of ordinary skill in the art, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.

[0168] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some or all of the technical features therein by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the technical solutions of the embodiments of the present invention.

Claims

1. A vehicle trajectory tracking method, characterized in that: Applied to a vehicle including a tractor and a towed device, the method comprises: Determine a device reference trajectory of the towed device within a preset time period; wherein the initial moment of the preset time period is the current moment; Determine the planned trajectory of the tractor within the preset time period according to the equipment reference trajectory and the relative position relationship between the tracking point and the connection point of the towed equipment; wherein the connection point is the connection point between the towed equipment and the tractor; Determine the tractor control sequence required to achieve the first preset goal by means of simulation tracking; wherein the tractor control sequence is a sequence formed by each tractor control quantity corresponding to each moment within the preset time period; the first preset goal is that the tractor can track the planned trajectory of the tractor; The tractor control sequence is sent to the chassis control system of the tractor, so that the chassis control system controls the driving of the tractor according to the tractor control sequence at the current moment, thereby realizing the trajectory tracking of the towed equipment at the current moment.

2. The method according to claim 1, characterized in that The device reference trajectory is a first device reference trajectory; correspondingly, determining the planned trajectory of the tractor within the preset time period according to the device reference trajectory and the relative position relationship between the tracking point and the connection point of the towed device includes: Determine the device state sequence required to achieve the second preset goal by means of simulation tracking; wherein the device state sequence is a sequence formed by the state quantities of the towed device corresponding to each moment in the preset time period, and the second preset goal is that the towed device can track the reference trajectory of the first device; Determining a corresponding second device reference trajectory according to the device state sequence; According to the reference trajectory of the second device and the relative position relationship between the tracking point and the connection point of the towed device, the connection point planning trajectory corresponding to the connection point is determined, and the connection point planning trajectory is used as the tractor planning trajectory.

3. The method according to claim 2, characterized in that The determining of the device state sequence required to achieve the second preset target by means of simulation tracking includes: S11, obtaining the towed device state quantity of the towed device at the current moment; S12, determining the control amount of the towed device required to achieve the second preset target at the current moment according to the state amount of the towed device at the current moment; S13, according to the state quantity of the towed device and the control quantity of the towed device at the current moment, and based on the bicycle kinematics model, simulate and track the next moment to obtain the state quantity of the towed device at the next moment; and use the next moment as the initial first moment; S14, determining whether the current first moment is the end moment of the preset time period; S15, if the current first moment is not the termination moment, determining the control amount of the towed device required to achieve the second preset target at the first moment according to the state amount of the towed device at the first moment; performing simulation tracking on the moment next to the first moment according to the state amount of the towed device and the control amount of the towed device at the first moment and based on the bicycle kinematics model, obtaining the state amount of the towed device at the moment next to the first moment; taking the moment next to the first moment as the first moment, and returning to S14; S16: If the current first moment is the termination moment, the state quantities of the towed equipment corresponding to each moment in the preset time period are formed into the equipment state sequence.

4. The method according to claim 3, characterized in that The towed device control amount includes the towed device front wheel angle and the towed device speed; correspondingly, the towed device control amount required at the current moment to achieve the second preset target according to the towed device state amount at the current moment includes: Determine the front wheel turning angle of the towed device required to achieve the second preset target at the current moment by using a pure tracking algorithm and the state quantity of the towed device at the current moment; The speed of the towed equipment required at the current moment to achieve the second preset target is determined through a proportional-integral-differential control algorithm.

5. The method according to claim 4, characterized in that The state quantity of the towed device includes the position coordinates of the towed device and the heading angle of the towed device; correspondingly, the front wheel turning angle of the towed device required to achieve the second preset target at the current moment determined by the pure tracking algorithm and the state quantity of the towed device at the current moment includes: Determine a pre-anchor point according to the position coordinates of the towed device and the reference trajectory of the first device; wherein the pre-anchor point is a point on the reference trajectory of the first device that is a preset distance away from the tracking point of the towed device; Determine, according to the heading angle of the towed device, an angle between a line connecting the pre-anchor point and the tracking point of the towed device and a body of the towed device; The front wheel turning angle of the towed equipment at the current moment is calculated according to the included angle.

6. The method according to claim 5, characterized in that The calculating the front wheel turning angle of the towed equipment at the current moment according to the included angle comprises: The first calculation formula is used to calculate the front wheel turning angle of the towed equipment, and the first calculation formula is: Among them, δ f is the front wheel turning angle of the towed equipment, L is the wheelbase between the front and rear axles of the towed equipment, and α is the angle; l d is the preset distance.

7. The method according to claim 1, characterized in that The method of determining the tractor control sequence required to achieve the first preset target by means of simulation tracking includes: S21, obtaining the tractor state quantity of the tractor at the current moment; S22, determining the tractor control amount required to achieve the first preset target at the current moment according to the tractor state amount at the current moment; S23, according to the tractor state quantity and the tractor control quantity at the current moment, and based on the bicycle kinematics model, simulate and track the next moment to obtain the tractor state quantity at the next moment; and use the next moment as the initial second moment; S24, determining whether the current second moment is the end moment of the preset time period; S25, if the current second moment is not the termination moment, determining the tractor control amount required to achieve the first preset target at the second moment according to the tractor state at the second moment; performing simulation tracking on the moment next to the second moment based on the bicycle kinematics model according to the tractor state and the tractor control at the second moment, to obtain the tractor state at the moment next to the second moment; taking the moment next to the second moment as the second moment, and returning to S24; S26. If the current second moment is the termination moment, the tractor control quantities corresponding to each moment in the preset time period are formed into the tractor control sequence.

8. A vehicle trajectory tracking device, characterized in that: Applicable to a vehicle including a tractor and a towed device, the device comprises: A first determination module is used to determine the device reference trajectory of the towed device within a preset time period; wherein the initial moment of the preset time period is the current moment; A second determination module is used to determine the planned trajectory of the tractor within the preset time period according to the equipment reference trajectory and the relative position relationship between the tracking point and the connection point of the towed equipment; wherein the connection point is the connection point between the towed equipment and the tractor; A first simulation module is used to determine the tractor control sequence required to achieve a first preset goal by means of simulation tracking; wherein the tractor control sequence is a sequence formed by each tractor control quantity corresponding to each moment within the preset time period; and the first preset goal is that the tractor can track the planned trajectory of the tractor; The control sending module is used to send the tractor control sequence to the chassis control system of the tractor, so that the chassis control system controls the driving of the tractor at the current moment according to the tractor control sequence, thereby realizing the trajectory tracking of the towed equipment at the current moment.

9. A computer-readable storage medium, characterized in that: A computer program is stored thereon, and when the computer program is executed in a computer, the computer is caused to execute the method according to any one of claims 1 to 7.

10. A computing device, characterized in that: The method comprises a memory and a processor, wherein the memory stores executable codes, and when the processor executes the executable codes, the method according to any one of claims 1 to 7 is implemented.

Images