CN117068185B

CN117068185B - Track vehicle track prediction method, track vehicle track prediction equipment and track vehicle track prediction medium

Info

Publication number: CN117068185B
Application number: CN202311344890.3A
Authority: CN
Inventors: 王昭建; 杨建森; 张辉; 张永康; 郭少杰; 吴德媛; 郝剑虹; 武振江
Original assignee: China Automotive Technology and Research Center Co Ltd; CATARC Tianjin Automotive Engineering Research Institute Co Ltd
Current assignee: China Automotive Technology and Research Center Co Ltd; CATARC Tianjin Automotive Engineering Research Institute Co Ltd
Priority date: 2023-10-18
Filing date: 2023-10-18
Publication date: 2024-01-02
Anticipated expiration: 2043-10-18
Also published as: CN117068185A

Abstract

The invention relates to the technical field of traffic control, and discloses a track prediction method, equipment and medium for a tracked vehicle, wherein the method comprises the following steps: the method comprises the steps of constructing a kinematic model and a transmission system model of the tracked vehicle, inputting transmission system parameters and steering wheel corners at the first moment in a current prediction period into the transmission system model to obtain left linear speed and right linear speed, further obtaining a track prediction model constructed based on the kinematic model, judging whether the tracked vehicle slides and deflects in a course or not based on high-precision combined navigation information of the tracked vehicle, correcting a calculation unit in the track prediction model if the tracked vehicle slides or deflects in a course, further determining predicted tracks of left and right tracks in the current prediction period according to the corrected track prediction model to obtain a more real predicted track, solving the problem that the predicted track precision is low due to the fact that the sliding and the deflecting in the course are not considered in the prior art, and improving the accuracy and safety of autonomous control of the tracked vehicle.

Description

Track vehicle track prediction method, track vehicle track prediction equipment and track vehicle track prediction medium

Technical Field

The invention relates to the technical field of traffic control, in particular to a track prediction method, equipment and medium for a tracked vehicle.

Background

Along with the continuous development of the automatic driving technology, the vehicle track prediction technology gradually becomes a hot spot, and an automatic driving algorithm performs path planning of the vehicle according to the predicted track information, makes a decision on possible dangerous situations, brakes or gives an alarm, and has important significance for improving the safety and stability of automatic driving.

The track vehicle is a special vehicle with wide application in civil and military fields, the rapid development of the automatic driving technology in the passenger vehicle field promotes the intelligent and unmanned development of the track vehicle, but the track prediction of the track vehicle is less studied at present, the existing track prediction method for the track vehicle does not consider that the track vehicle can slide and the lateral deviation of the car body course can occur when the track vehicle turns, the predicted track precision of the track vehicle is low, and therefore errors are brought to subsequent decisions and path planning, and the accuracy and safety of autonomous control of the track vehicle are reduced.

In view of this, the present invention has been made.

Disclosure of Invention

In order to solve the technical problems, the invention provides a track prediction method, equipment and medium for a tracked vehicle, which solve the problem that the track prediction precision is low due to the fact that the tracked vehicle slides and the lateral deviation of the body course occurs during steering in the prior art, and improve the accuracy and safety of autonomous control of the tracked vehicle.

The embodiment of the invention provides a track prediction method for a tracked vehicle, which comprises the following steps:

constructing a kinematic model and a transmission system model of the tracked vehicle, and inputting transmission system parameters and steering wheel angles of the tracked vehicle at the first moment in a current prediction period into the transmission system model to obtain a left linear speed and a right linear speed;

acquiring a track prediction model constructed based on the kinematic model, wherein the track prediction model comprises a first calculation unit, a second calculation unit and a third calculation unit, the first calculation unit is used for determining a predicted yaw rate and a predicted central line rate according to the left linear rate and the right linear rate, the second calculation unit is used for determining a predicted course angle of a next moment according to the predicted course angle and the predicted yaw rate of one moment, and the third calculation unit is used for determining positions of a left crawler belt and a right crawler belt at the next moment according to the predicted central line rate of the moment, the vehicle position and the predicted course angle of the next moment;

and judging whether the tracked vehicle has track sliding and course lateral deviation based on the high-precision combined navigation information of the tracked vehicle, if so, correcting a calculation unit in the track prediction model, and determining the predicted track of the left track and the right track of the tracked vehicle in the current prediction period based on the corrected track prediction model.

The embodiment of the invention provides electronic equipment, which comprises:

a processor and a memory;

the processor is configured to execute the steps of the tracked vehicle trajectory prediction method according to any one of the embodiments by calling a program or instructions stored in the memory.

Embodiments of the present invention provide a computer-readable storage medium storing a program or instructions that cause a computer to perform the steps of the tracked vehicle trajectory prediction method according to any one of the embodiments.

The embodiment of the invention has the following technical effects:

the method comprises the steps of constructing a kinematic model and a transmission system model of a tracked vehicle, inputting transmission system parameters and steering wheel angles of the tracked vehicle at the first moment in a current prediction period to the transmission system model, obtaining left linear speed and right linear speed, further obtaining a track prediction model constructed based on the kinematic model, judging whether the tracked vehicle slides and deflects in a course based on high-precision combined navigation information of the tracked vehicle, correcting a calculation unit in the track prediction model if the tracked vehicle slides or deflects in a course, further determining predicted tracks of the left tracked vehicle and the right tracked vehicle in the current prediction period according to the corrected track prediction model, recognizing by considering the sliding of the tracked vehicle and the deflection of a vehicle body course, correcting the track prediction model by combining with high-precision combined navigation information, enabling the track prediction precision of the corrected model to be higher, obtaining a more real predicted track, solving the problem that the predicted track precision is low due to the fact that the tracked vehicle slides and the deflection of the vehicle body course is not considered in the steering in the prior art.

Drawings

In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings that are needed in the description of the embodiments or the prior art will be briefly described, and it is obvious that the drawings in the description below are some embodiments of the present invention, and other drawings can be obtained according to the drawings without inventive effort for a person skilled in the art.

FIG. 1 is a flowchart of a track vehicle trajectory prediction method provided by an embodiment of the present invention;

FIG. 2 is a schematic view of basic dimensional parameters of a tracked vehicle according to an embodiment of the present invention;

FIG. 3 is a schematic diagram of a track prediction result according to an embodiment of the present invention;

fig. 4 is a schematic structural diagram of an electronic device according to an embodiment of the present invention.

Detailed Description

In order to make the objects, technical solutions and advantages of the present invention more apparent, the technical solutions of the present invention will be clearly and completely described below. It will be apparent that the described embodiments are only some, but not all, embodiments of the invention. All other embodiments, which can be made by one of ordinary skill in the art without undue burden from the invention, are within the scope of the invention.

The track prediction method for the tracked vehicle is mainly suitable for track prediction of the tracked vehicle. The track vehicle track prediction method provided by the embodiment of the invention can be executed by the electronic equipment in the integrated automatic driving vehicle.

Fig. 1 is a flowchart of a track vehicle track prediction method provided in an embodiment of the present invention. Referring to fig. 1, the track vehicle track prediction method specifically includes:

s110, constructing a kinematic model and a transmission system model of the tracked vehicle, and inputting transmission system parameters and steering wheel angles of the tracked vehicle at the first moment in a current prediction period into the transmission system model to obtain left linear speed and right linear speed.

The tracked vehicle can be a vehicle with a tracked running system instead of a wheel running system, such as a tracked carrier, a tracked mountain bike and the like. The kinematic model may be a model of an ideal tracked vehicle that does not take into account track slip and body heading yaw.

Specifically, basic size parameters of the tracked vehicle can be obtained first, and a kinematic model of the tracked vehicle can be built through the basic size parameters. Fig. 2 is a schematic view illustrating basic dimensional parameters of a tracked vehicle according to an embodiment of the present invention, as shown in fig. 2, For centre distance of tracked vehicle, < >>For the linear speed of the left track, i.e. left linear speed,/->Is the linear speed of the right track, i.e. right linear speed,/->Is the linear speed of the centre of the track, i.e. the geometric centre of the tracked vehicle, +.>Yaw rate for tracked vehicle, +.>For the turning radius of a tracked vehicle +.>Is the geometric center of the tracked vehicle, < - > a->Is the center of rotation of the tracked vehicle, < >>Is the world coordinate system>And->In world coordinates for tracked vehicles +.>Shaft position and shaft positionPosition of shaft->Is the heading angle of the tracked vehicle.

Referring to fig. 2, using the triangle similarity principle, the following equation can be obtained:

；

the meaning of each symbol in the formula can be referred to in the previous discussion, and further, the turning radius R of the tracked vehicle can be calculated according to the formula as follows:

；

as can be seen from FIG. 2, the linear velocity of the track centerThe method comprises the following steps:

；

further, the yaw rate of the tracked vehicle can be obtainedThe method comprises the following steps:

；

wherein the course angle and the yaw rate may be set to be positive when the vehicle turns left, and negative when the vehicle turns right. Further, an ideal kinematic model of the tracked vehicle can be constructed according to the formula:

；

wherein, For the derivative of the abscissa of the tracked vehicle in world coordinate system, i.e. the speed in the abscissa direction in world coordinate system +.>，/>For the derivative of the longitudinal coordinate of the tracked vehicle in world coordinate system, i.e. speed +.>，/>For the derivative of the course angle of the tracked vehicle, i.e. yaw rate +.>。

In an embodiment of the invention, the driveline model may be used to output left and right linear speeds of the tracked vehicle in accordance with the entered driveline parameters. The driveline parameters may include, among other things, left side track input shaft speed, right side track input shaft speed, current gear, and radius of the track drive wheel. Since the engine torque of the tracked vehicle is transmitted to the drive wheels, the input shaft speed of the track can be understood as the speed of the engine output shaft. The current gear may be a forward gear, a reverse gear, a steering gear, or a neutral gear.

Specifically, the method provided by the embodiment of the invention can realize periodic track prediction, for example, the prediction step length can be set to be 0.1s, the prediction step number is 10, the prediction period can be 1s, the prediction period comprises 10 moments, and the interval between two adjacent moments is 0.1s. When the track in one prediction period is started to be predicted, the transmission system parameter and the steering wheel angle at the first moment (namely the current moment) in the current prediction period can be input into the transmission system model so as to obtain the left linear speed and the right linear speed at the first moment in the current prediction period.

In a specific embodiment, the transmission system parameters and steering wheel angles of the tracked vehicle at the first moment in the current prediction period are input into a transmission system model to obtain a left linear speed and a right linear speed, and the method comprises the following steps: if the current gear in the transmission system parameter at the first moment is a neutral gear and the steering wheel rotation angle at the first moment is larger than zero, determining that the left linear velocity is zero and determining the right linear velocity according to the steering wheel rotation angle; or if the current gear in the transmission system parameter at the first moment is a neutral gear and the steering wheel angle at the first moment is smaller than zero, determining that the right linear velocity is zero, and determining the left linear velocity according to the steering wheel angle.

That is, if the input current gear is neutral and the steering wheel angle is greater than zero, it means that the vehicle is stationary at this time and the steering angle has been input, and since the steering wheel angle is greater than zero (the steering wheel angle and yaw rate are greater than zero when steering to the left are preset), it may be determined that the left linear velocity is zero at this time, and the right linear velocity may be calculated from the steering wheel angle, by way of example, the following formula may be adopted:

；

wherein,is steering wheel angle>、/>Left and right linear velocities, respectively.

If the input current gear is neutral and the steering wheel angle is smaller than zero, it indicates that the vehicle is stationary at this time and the steering angle has been input, and since the steering wheel angle is smaller than zero, it can be determined that the right linear velocity is zero at this time, and the left linear velocity can be calculated from the steering wheel angle, for example, the following formula can be adopted:

；

it should be noted that, in the above formula,and +.>The method can be calibrated in advance according to actual acquired data, and the embodiment of the invention does not limit a calculation formula between the left linear speed, the right linear speed and the steering wheel rotation angle. By the method, the left linear velocity and the right linear velocity at the first moment in the current prediction period can be accurately determined, and therefore accuracy of track prediction is guaranteed.

In addition to the above, if the current gear in the first-time driveline parameter is the forward gear and the first-time steering wheel angle is zero, it indicates that the vehicle is not inputting a steering angle, and at this time, the left and right linear speeds may be calculated by the following formula:

；

in the method, in the process of the invention,for the radius of the track drive wheel +.>、/>The left crawler belt input shaft rotating speed and the right crawler belt input shaft rotating speed are respectively +. >For the gear ratio corresponding to the current gear, +.>Is the rotational speed of the drive wheel.

If the current gear in the transmission system parameter at the first moment is a reverse gear and the steering wheel angle at the first moment is zero, the vehicle is indicated to not input a steering angle, and at the moment, the left linear speed and the right linear speed can be calculated through the following formulas:

；

if the current gear in the transmission system parameter at the first moment is a steering gear and the steering wheel angle at the first moment is not equal to zero, the left linear speed and the right linear speed can be calculated according to the following formula:

；

if the current gear in the transmission system parameter at the first moment is a neutral gear and the steering wheel angle at the harvesting moment is zero, the left linear speed and the right linear speed can be calculated through the following formulas:

。

s120, acquiring a track prediction model constructed based on the kinematic model.

The track prediction model comprises a first calculation unit, a second calculation unit and a third calculation unit, wherein the first calculation unit is used for determining a predicted yaw rate and a predicted central line speed according to a left linear speed and a right linear speed, the second calculation unit is used for determining a predicted course angle of the next moment according to a predicted course angle and a predicted yaw rate of one moment, and the third calculation unit is used for determining positions of the left crawler belt and the right crawler belt at the next moment according to the predicted central line speed of the moment, the vehicle position and the predicted course angle of the next moment.

The track prediction model can be constructed by a kinematic model. Specifically, the first calculation unit may be expressed by the following formula:

；

in the method, in the process of the invention,、/>the predicted centerline speed and the predicted yaw rate may be the same at each time in the current prediction period, or the predicted yaw rate may be the same at each time.

Wherein the second calculation unit may be expressed by the following formula:

；

in the method, in the process of the invention,、/>for a predicted course angle and a predicted yaw rate at one instant, +.>For the predicted course angle at the next time, for example, the predicted course angle at the first time in the current prediction period may be input into the formula to obtain the predicted course angle at the second time, the predicted course angle at the second time may be input into the formula to obtain the predicted course angle at the third time, and so on, the predicted course angle at each time may be obtained. The predicted course angle at the first time in the current prediction period may be the predicted course angle at the last time in the previous prediction period, or may be a course angle obtained from high-precision integrated navigation information corresponding to the first time in the current prediction period.

The third calculation unit may include a speed prediction subunit, a position prediction subunit, a heading angle error subunit, a left track position prediction subunit, and a right track position prediction subunit. The speed predictor unit is configured to determine a vehicle speed at a next time based on the predicted centerline speed at the one time and the predicted heading angle at the next time, and may be expressed by the following formula:

；

in the method, in the process of the invention,、/>the speed of the tracked vehicle in the abscissa and the speed of the tracked vehicle in the ordinate of the world coordinate system at the next moment are respectively. For example, the predicted centerline speed at the first time and the predicted centerline speed at the second time within the current prediction period may be calculatedThe measured heading angle is input into the formula to obtain the vehicle speed at the second moment, and the like to obtain the vehicle speed at each moment.

The position prediction subunit is configured to determine a vehicle position at a next time according to the vehicle position at the one time and the vehicle speed at the next time. Illustratively, the position predictor unit may be represented by the following formula:

；

in the method, in the process of the invention,、/>the position of the tracked vehicle in the abscissa direction and the position of the tracked vehicle in the ordinate direction of the world coordinate system at the next moment are respectively. For example, the vehicle position at the first time and the vehicle speed at the second time in the current prediction period may be input into the above formula to obtain the vehicle position at the second time, and so on to obtain the vehicle positions at the respective times.

The course angle error subunit is used for determining the course angle error between adjacent moments according to the predicted course angle of one moment and the predicted course angle of the next moment. Exemplary, tracked vehicle geometric center at the present momentCAs origin, take the longitudinal direction of the vehicle body asyThe shaft takes the lateral direction of the vehicle body asxShaft for establishing local coordinate system of vehicle bodyC-x _s -y _s The heading angle error subunit may be expressed by the following formula:

；

in the method, in the process of the invention,for a predicted heading angle of one moment, +.>For the predicted heading angle of the next moment, < +.>Is the heading angle error between adjacent moments. For example, the predicted course angle at the first time and the predicted course angle at the second time in the current prediction period may be input into the above formula, to obtain a course angle error between the first time and the second time, and so on, to obtain a course angle error between adjacent times.

The left track position prediction subunit is used for determining the position of the left track at one moment according to the position and course angle error of the vehicle at one moment. For example, the left track position prediction subunit may be expressed by the following formula:

；

in the method, in the process of the invention,、/>the next vehicle position output for the position prediction subunit, +. >For the center distance of the crawler belt,for the course angle error corresponding to the next moment output by the course angle error subunit, +.>、/>The position of the left crawler of the crawler vehicle in the abscissa direction and the position in the ordinate direction in the world coordinate system are the positions of the left crawler of the crawler vehicle at this time. For example, the vehicle position at the second time, the heading error between the second time and the first time may be input to the above equation,the position of the left track at the second moment is obtained, and so on, and the position of the left track at each moment is obtained.

The right track position prediction subunit is used for determining the position of the right track at one moment according to the position and course angle error of the vehicle at one moment. For example, the right track position prediction subunit may be expressed by the following formula:

；

in the method, in the process of the invention,、/>the next vehicle position output for the position prediction subunit, +.>、/>The position of the right crawler of the crawler vehicle in the abscissa direction and the position in the ordinate direction in the world coordinate system are the positions of the right crawler of the crawler vehicle at this time. For example, the vehicle position at the second time, the heading error between the second time and the first time may be input to the above formula to obtain the position of the right track at the second time, and so on to obtain the position of the right track at each time.

S130, judging whether the tracked vehicle has track sliding and course lateral deviation based on high-precision combined navigation information of the tracked vehicle, if so, correcting a computing unit in a track prediction model, and determining predicted tracks of the left track and the right track of the tracked vehicle in a current prediction period based on the corrected track prediction model.

In the embodiment of the invention, the situation that the track slides when the tracked vehicle turns is considered, such as the situation that the track slides on a high-speed side track and the track slides on a low-speed side track, and meanwhile, the situation that the vehicle body deflects in course is considered, so that the track prediction precision of the vehicle is affected, and the track prediction model is a prediction model which does not consider the track slide and the course deflection, so that the situation of the track slide and the course deflection can be identified through high-precision combined navigation information, and the track prediction model is corrected when the situation of the track slide or the course deflection exists, so that the track precision of model prediction is ensured.

In order to avoid abrupt change and fluctuation of the predicted track caused by signal fluctuation in the high-precision integrated navigation information, the high-precision integrated navigation information can be subjected to extended Kalman filtering to obtain a smooth signal curve, and then information such as an actual yaw rate, an actual course angle, an east speed, a north speed and the like at the first moment in the current prediction period is obtained from the high-precision integrated navigation information.

In a specific embodiment, the high-precision integrated navigation information includes an actual yaw rate, an east speed and a north speed of the tracked vehicle at a first moment in a current prediction period, and determining whether the tracked vehicle performs track sliding based on the high-precision integrated navigation information of the tracked vehicle includes: determining an actual center line speed of the tracked vehicle based on the east and north speeds, and determining an actual turning radius of the tracked vehicle based on the actual center line speed and the actual yaw rate; determining a left relative speed of a left track and a right relative speed of a right track in the tracked vehicle based on the actual turning radius and the actual yaw rate; and judging whether the tracked vehicle slides on the tracks according to the left relative speed, the left linear speed, the right relative speed and the right linear speed.

The east speed may be a speed in an abscissa direction in the world coordinate system, and the north speed may be a speed in an ordinate direction in the world coordinate system. Specifically, the actual center line speed may be calculated by combining the east and north directions in the navigation information with high accuracy, for example, the actual center line speed may be calculated by the following formula:

；

In the method, in the process of the invention,、/>north speed, east speed, < >>Is the actual centerline speed. Further, the actual turning radius of the tracked vehicle may be determined by the actual centerline speed and the actual yaw rate in the high-precision integrated navigation information, for example:

；

in the method, in the process of the invention,for the actual yaw rate +.>Is the actual turning radius. Further, the actual turning radius of the left high-speed track and the actual turning radius of the right high-speed track may be determined according to the actual turning radius, for example:

，/>；

in the method, in the process of the invention,for the actual radius of rotation of the left high-speed track, < > for the left high-speed track>Is the actual radius of rotation of the right high speed track. Further, the left relative speed of the left high-speed track can be determined according to the actual rotation radius of the left high-speed track, and the left relative speed of the left high-speed track can be determined according to the actual rotation half of the right high-speed trackThe diameter determines the right relative speed of the right track, for example:

，/>；

in the method, in the process of the invention,、/>the left relative speed of the left track and the right relative speed of the right track, respectively.

Further, whether the tracked vehicle slides on the track can be judged through the left relative speed and the right relative speed, and the right linear speed and the left linear speed output by the transmission system model. By the method, accurate identification of track sliding is achieved, and accuracy of track prediction is further guaranteed.

In one example, determining whether track slippage of the tracked vehicle occurs based on the left relative speed, the left linear speed, the right relative speed, and the right linear speed includes: determining an absolute value corresponding to a ratio between the left relative speed and the left linear speed as a left track coefficient, and determining an absolute value corresponding to a ratio between the right relative speed and the right linear speed as a right track coefficient; if the left track coefficient and the right track coefficient are both larger than the preset slip threshold, or the left track coefficient and the right track coefficient are both smaller than the preset slip threshold, or the left track coefficient is larger than the preset slip threshold and the right track coefficient is smaller than the preset slip threshold, or the left track coefficient is smaller than the preset slip threshold and the right track coefficient is larger than the preset slip threshold, determining that the tracked vehicle slides.

Specifically, an absolute value corresponding to a ratio between the left relative speed and the left linear speed may be calculated to obtain a left track coefficient, and an absolute value corresponding to a ratio between the right relative speed and the right linear speed may be calculated to obtain a right track coefficient. Further, the left track coefficient and the right track coefficient can be compared with a preset slip threshold and a preset slip threshold.

If the left-side track coefficient and the right-side track coefficient are both larger than the preset slip threshold, the left-side track and the right-side track simultaneously slip, and the tracked vehicle slides; if the left side track coefficient and the right side track coefficient are smaller than the preset slippage threshold, the left side track and the right side track are simultaneously slipped, and the tracked vehicle is slipped; if the left side track coefficient is larger than the preset slip threshold value and the right side track coefficient is smaller than the preset slip threshold value, the left side track is indicated to slip, the right side track is indicated to slip, and the tracked vehicle is indicated to slip; and if the left side track coefficient is smaller than the preset slip threshold value and the right side track coefficient is larger than the preset slip threshold value, the left side track is indicated to slip, the right side track is indicated to slip, and the tracked vehicle is indicated to slip. In addition to the above, it can be considered that neither the left nor the right tracks slip.

By means of the mode that the absolute value of the ratio is compared with the preset slip threshold value and the preset slip threshold value, accurate identification of track slip can be achieved, and then the track prediction model is corrected when the track slips, so that accuracy of track prediction is guaranteed.

Optionally, the correcting the computing unit in the track prediction model includes: in the case of track slip of the tracked vehicle, the first calculation unit is modified to determine a predicted yaw rate from the actual yaw rate and a predicted centerline speed from the actual centerline speed.

That is, if the tracked vehicle is subjected to track slip, the first calculation unit is modified to determine the actual yaw rate as the predicted yaw rate, and the actual center line speed as the predicted center line speed. The modified first calculation unit may be expressed by the following formula:

，/>；

in the method, in the process of the invention,for predicting yaw rate +.>To predict centerline speed, +.>For predicting yaw rate +.>Is the actual centerline speed. By the method, the track prediction model is corrected under the condition of track sliding, and the track prediction accuracy is ensured.

Further, the predicted yaw rate and the predicted center line speed can be determined through the first modified computing unit, then the predicted yaw rate and the predicted center line speed are input into the second computing unit to obtain a predicted course angle at the next moment, then the predicted course angle at the next moment is input into the third computing unit to obtain the positions of the left crawler belt and the right crawler belt at the next moment, then the next moment is taken as the current moment, the steps are circulated, the positions of the left crawler belt and the right crawler belt at all moments in the current prediction period are obtained, and further the predicted track of the left crawler belt and the right crawler belt in the current prediction period is obtained.

In a specific embodiment, the high-precision combined navigation information includes an actual course angle, an actual yaw rate, an east direction speed and a north direction speed of the tracked vehicle at a first moment in a current prediction period, and the determining whether the tracked vehicle has course cornering based on the high-precision combined navigation information of the tracked vehicle includes: determining an actual speed heading of the tracked vehicle based on the east speed and the north speed; and determining the slip angle of the tracked vehicle according to the actual speed course and the actual course angle, and determining that the tracked vehicle is subjected to course slip if the absolute value corresponding to the slip angle is larger than a preset angle.

Specifically, the actual speed heading can be calculated by the following formula:

；

in the method, in the process of the invention,for the actual speed heading, +.>、/>North speed and east speed respectively. The slip angle of the tracked vehicle can be calculated by the following formula:

；

in the method, in the process of the invention,is the cornering angle->For the actual speed heading, +.>The actual course angle in the high-precision combined navigation information is obtained. Further, the absolute value of the slip angle can be compared with a preset angle, and if the absolute value of the slip angle is larger than the preset angle, the fact that the tracked vehicle is in heading slip can be determined. By the method, accurate identification of the vehicle body course lateral deviation of the tracked vehicle can be achieved, and then the track prediction model is corrected when the vehicle body course lateral deviation is achieved, so that the track prediction accuracy is guaranteed.

Optionally, the correcting the computing unit in the track prediction model includes: and under the condition that the tracked vehicle is subjected to course lateral deviation, the second calculation unit is corrected to determine the predicted course angle of the next moment according to the actual speed course and the actual yaw rate of the moment.

That is, if the tracked vehicle is subject to yaw misalignment, the second calculation unit is modified to determine the predicted heading angle at the next moment from the actual yaw rate and the actual speed heading in the high-precision integrated navigation information. The modified second calculation unit may be expressed by the following formula:

；

in the method, in the process of the invention,for the actual yaw rate at one instant, < +.>For the actual speed course at one instant, +.>And the predicted course angle for the next moment. By the method, the track prediction model is corrected under the condition of vehicle body course lateral deviation, and the track prediction accuracy is ensured.

Further, the predicted yaw rate and the predicted center line speed can be determined through the first computing unit, then the predicted course angle at the next moment is obtained through the second computing unit after modification, then the predicted course angle at the next moment is input into the third computing unit, the positions of the left crawler belt and the right crawler belt at the next moment are obtained, the next moment is taken as the current moment, the steps are circulated, the positions of the left crawler belt and the right crawler belt at all moments in the current prediction period are obtained, and the predicted track of the left crawler belt and the right crawler belt in the current prediction period is obtained.

In one example, determining a predicted track for a left track and a right track of a tracked vehicle over a current predicted period based on a modified track prediction model includes: inputting the high-precision combined navigation information, the left linear velocity, the right linear velocity, the predicted course angle of the tracked vehicle at the first moment in the current prediction period and the vehicle position into a track prediction model to obtain the positions of the left track and the right track output by the track prediction model at all moments in the current prediction period; fitting the positions of the left crawler belt at all times in the current prediction period to obtain a prediction track of the left crawler belt in the current prediction period, and fitting the positions of the right crawler belt at all times in the current prediction period to obtain a prediction track of the right crawler belt in the current prediction period.

The predicted course angle and the vehicle position of the tracked vehicle at the first moment in the current prediction period can be the predicted course angle and the vehicle position at the last moment in the last prediction period, and can also be the course angle and the vehicle position obtained from the high-precision combined navigation information.

Specifically, if it is determined that the tracked vehicle does not have track slip and heading yaw, a track prediction model may be used to determine predicted tracks of the left track and the right track in the current prediction period, and if it is determined that the tracked vehicle has track slip and heading yaw, the first computing unit and the second computing unit may be modified, so that the predicted tracks of the left track and the right track in the current prediction period are determined based on the modified track prediction model.

For example, taking 10 moments included in the current prediction period as an example, when no track slip and no heading bias occur in the tracked vehicle, the left linear speed and the right linear speed (which are assumed to be the same at all moments in the current prediction period) output by the transmission system model may be input to the first calculation unit to obtain a predicted yaw rate and a predicted centerline speed (which are assumed to be the same at all moments in the current prediction period), then the predicted yaw rate and the predicted centerline speed are input to the second calculation unit to obtain a predicted heading angle at the second moment, and then the predicted centerline speed, the vehicle position at the first moment, and the predicted heading angle at the second moment are input to the third calculation unit to obtain positions of the left track and the right track at the second moment, and the steps are cycled until positions of the left track and the right track at all moments are obtained.

When the tracked vehicle slides and does not have course lateral deviation, the actual yaw rate and the actual central line speed at the first moment in the current prediction period in the high-precision combined navigation information can be input into a first calculation unit after modification to obtain the predicted yaw rate and the predicted central line speed, the predicted yaw rate and the predicted central line speed are further input into a second calculation unit to obtain the predicted course angle at the second moment, the predicted central line speed, the vehicle position at the first moment and the predicted course angle at the second moment are further input into a third calculation unit to obtain the positions of the left track and the right track at the second moment, and the steps are circulated until the positions of the left track and the right track at all moments are obtained.

When the tracked vehicle is laterally deviated in course and is not slid in the track, the left linear speed and the right linear speed which are output by the transmission system model can be input into the first calculation unit to obtain the predicted yaw rate and the predicted central line speed, then the actual yaw rate and the actual speed course in the high-precision combined navigation information are input into the corrected second calculation unit to obtain the predicted course angle at the second moment, and then the predicted central line speed, the vehicle position at the first moment and the predicted course angle at the second moment are input into the third calculation unit to obtain the positions of the left track and the right track at the second moment, and the steps are circulated until the positions of the left track and the right track at all moments are obtained.

Illustratively, the sequence of positions of the left track obtained during the current prediction period is:，/>the method comprises the steps of carrying out a first treatment on the surface of the The position sequence of the right caterpillar band is as follows: />，。/>

Further, the positions of the left crawler belt in the current prediction period at all times can be fitted, and the positions of the right crawler belt in the current prediction period at all times can be fitted, so that the predicted track of the left crawler belt and the predicted track of the right crawler belt can be obtained.

For example, a least squares fit of the polynomial may be performed three times based on the position at each instant, with each trajectory yielding four polynomial parameters. Polynomial parameters of left crawler And polynomial parameters of right track +.>The method comprises the following steps of:

，/>。

by the method, the track can be fitted, the accuracy of the track is further ensured, and the follow-up operations such as path planning and the like according to the predicted track are facilitated.

For example, fig. 3 is a schematic diagram of a track prediction result provided by the embodiment of the present invention, as shown in fig. 3, the ideal track of the left track and the ideal track of the right track may be predicted based on an uncorrected track prediction model, the corrected track of the left track and the corrected track of the right track may be predicted based on a corrected track prediction model, and as can be seen from the figure, the corrected track better conforms to the actual running situation of the vehicle, and the accuracy is higher.

The invention has the following technical effects: the method comprises the steps of constructing a kinematic model and a transmission system model of a tracked vehicle, inputting transmission system parameters and steering wheel angles of the tracked vehicle at the first moment in a current prediction period to the transmission system model, obtaining left linear speed and right linear speed, further obtaining a track prediction model constructed based on the kinematic model, judging whether the tracked vehicle slides and deflects in a course based on high-precision combined navigation information of the tracked vehicle, correcting a calculation unit in the track prediction model if the tracked vehicle slides or deflects in a course, further determining predicted tracks of the left tracked vehicle and the right tracked vehicle in the current prediction period according to the corrected track prediction model, recognizing by considering the sliding of the tracked vehicle and the deflection of a vehicle body course, correcting the track prediction model by combining with high-precision combined navigation information, enabling the track prediction precision of the corrected model to be higher, obtaining a more real predicted track, solving the problem that the predicted track precision is low due to the fact that the tracked vehicle slides and the deflection of the vehicle body course is not considered in the steering in the prior art.

Fig. 4 is a schematic structural diagram of an electronic device according to an embodiment of the present invention. As shown in fig. 4, electronic device 400 includes one or more processors 401 and memory 402.

The processor 401 may be a Central Processing Unit (CPU) or other form of processing unit having data processing capabilities and/or instruction execution capabilities and may control other components in the electronic device 400 to perform desired functions.

Memory 402 may include one or more computer program products that may include various forms of computer-readable storage media, such as volatile memory and/or non-volatile memory. The volatile memory may include, for example, random Access Memory (RAM) and/or cache memory (cache), and the like. The non-volatile memory may include, for example, read Only Memory (ROM), hard disk, flash memory, and the like. One or more computer program instructions may be stored on the computer readable storage medium that can be executed by the processor 401 to implement the tracked vehicle track prediction method and/or other desired functions of any of the embodiments of the present invention described above. Various content such as initial arguments, thresholds, etc. may also be stored in the computer readable storage medium.

In one example, the electronic device 400 may further include: an input device 403 and an output device 404, which are interconnected by a bus system and/or other forms of connection mechanisms (not shown). The input device 403 may include, for example, a keyboard, a mouse, and the like. The output device 404 may output various information to the outside, including early warning prompt information, braking force, etc. The output device 404 may include, for example, a display, speakers, a printer, and a communication network and remote output devices connected thereto, etc.

Of course, only some of the components of the electronic device 400 that are relevant to the present invention are shown in fig. 4 for simplicity, components such as buses, input/output interfaces, etc. are omitted. In addition, electronic device 400 may include any other suitable components depending on the particular application.

In addition to the methods and apparatus described above, embodiments of the invention may also be a computer program product comprising computer program instructions which, when executed by a processor, cause the processor to perform the steps of a tracked vehicle track prediction method provided by any of the embodiments of the invention.

The computer program product may write program code for performing operations of embodiments of the present invention in any combination of one or more programming languages, including an object oriented programming language such as Java, C++ or the like and conventional procedural programming languages, such as the "C" programming language or similar programming languages. The program code may execute entirely on the user's computing device, partly on the user's device, as a stand-alone software package, partly on the user's computing device, partly on a remote computing device, or entirely on the remote computing device or server.

Furthermore, embodiments of the present invention may also be a computer-readable storage medium, having stored thereon computer program instructions, which when executed by a processor, cause the processor to perform the steps of the tracked vehicle track prediction method provided by any of the embodiments of the present invention.

The computer readable storage medium may employ any combination of one or more readable media. The readable medium may be a readable signal medium or a readable storage medium. The readable storage medium may include, for example, but is not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or a combination of any of the foregoing. More specific examples (a non-exhaustive list) of the readable storage medium would include the following: an electrical connection having one or more wires, a portable disk, a hard disk, random Access Memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing.

It is noted that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to limit the scope of the present application. As used in this specification, the terms "a," "an," "the," and/or "the" are not intended to be limiting, but rather are to be construed as covering the singular and the plural, unless the context clearly dictates otherwise. The terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, or apparatus. Without further limitation, an element defined by the phrase "comprising one … …" does not exclude the presence of other like elements in a process, method or apparatus comprising such elements.

It should also be noted that the positional or positional relationship indicated by the terms "center", "upper", "lower", "left", "right", "vertical", "horizontal", "inner", "outer", etc. are based on the positional or positional relationship shown in the drawings, are merely for convenience of describing the present invention and simplifying the description, and do not indicate or imply that the apparatus or element in question must have a specific orientation, be constructed and operated in a specific orientation, and thus should not be construed as limiting the present invention. Unless specifically stated or limited otherwise, the terms "mounted," "connected," and the like are to be construed broadly and may be, for example, fixedly connected, detachably connected, or integrally connected; can be mechanically or electrically connected; can be directly connected or indirectly connected through an intermediate medium, and can be communication between two elements. The specific meaning of the above terms in the present invention will be understood in specific cases by those of ordinary skill in the art.

Finally, it should be noted that: the above embodiments are only for illustrating the technical solution of the present invention, and not for limiting the same; although the invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical scheme described in the foregoing embodiments can be modified or some or all of the technical features thereof can be replaced by equivalents; such modifications and substitutions do not depart from the essence of the corresponding technical solutions from the technical solutions of the embodiments of the present invention.

Claims

1. A method of track vehicle trajectory prediction, comprising:

2. The method of claim 1, wherein inputting the driveline parameters and steering wheel angle of the tracked vehicle at the first instance in the current predicted period to the driveline model results in a left linear speed and a right linear speed, comprising:

if the current gear in the transmission system parameter at the first moment is a neutral gear and the steering wheel angle at the first moment is larger than zero, determining that the left linear speed is zero, and determining the right linear speed according to the steering wheel angle; or,

and if the current gear in the transmission system parameter at the first moment is a neutral gear and the steering wheel rotation angle at the first moment is smaller than zero, determining that the right linear velocity is zero, and determining the left linear velocity according to the steering wheel rotation angle.

3. The method of claim 1, wherein the high-precision integrated navigation information includes an actual yaw rate, an east speed, and a north speed of the tracked vehicle at a first time in the current prediction period, and determining whether the tracked vehicle is subject to track slip based on the high-precision integrated navigation information of the tracked vehicle comprises:

Determining an actual centerline speed of the tracked vehicle based on the east speed and the north speed, and determining an actual turning radius of the tracked vehicle based on the actual centerline speed and the actual yaw rate;

determining a left relative speed of a left track and a right relative speed of a right track in the tracked vehicle based on the actual turning radius and the actual yaw rate;

and judging whether the tracked vehicle slides on the tracks according to the left relative speed, the left linear speed, the right relative speed and the right linear speed.

4. The method of claim 3, wherein said determining whether track slippage of the tracked vehicle occurs based on the left relative speed, the left linear speed, the right relative speed, and the right linear speed comprises:

determining an absolute value corresponding to a ratio between the left relative speed and the left linear speed as a left track coefficient, and determining an absolute value corresponding to a ratio between the right relative speed and the right linear speed as a right track coefficient;

if the left track coefficient and the right track coefficient are both greater than a preset slip threshold, or the left track coefficient and the right track coefficient are both less than a preset slip threshold, or the left track coefficient is greater than the preset slip threshold and the right track coefficient is less than the preset slip threshold, or the left track coefficient is less than the preset slip threshold and the right track coefficient is greater than the preset slip threshold, determining that the tracked vehicle slides.

5. A method according to claim 3, wherein said modifying a computational unit in said trajectory prediction model comprises:

in the case where the tracked vehicle is subjected to track slip, the first calculation unit is modified to determine a predicted yaw rate from the actual yaw rate and a predicted centerline rate from the actual centerline rate.

6. The method of claim 1, wherein the high-precision integrated navigation information includes an actual heading angle, an actual yaw rate, an east speed, and a north speed of the tracked vehicle at a first time in the current prediction period, and determining whether the tracked vehicle is heading sideways based on the high-precision integrated navigation information of the tracked vehicle comprises:

determining an actual speed heading of the tracked vehicle based on the east speed and the north speed;

and determining a slip angle of the tracked vehicle according to the actual speed course and the actual course angle, and determining that the tracked vehicle is subjected to course slip if the absolute value corresponding to the slip angle is larger than a preset angle.

7. The method of claim 6, wherein said modifying a computational unit in said trajectory prediction model comprises:

And under the condition that the tracked vehicle is laterally deviated, the second calculation unit is modified to determine the predicted course angle of the next moment according to the actual speed course and the actual yaw rate of the moment.

8. The method of claim 1, wherein the determining a predicted track for the left and right tracks of the tracked vehicle for the current predicted period based on the modified track prediction model comprises:

inputting the high-precision integrated navigation information, the left linear velocity, the right linear velocity and the predicted course angle and the vehicle position of the tracked vehicle at the first moment in the current prediction period into the track prediction model to obtain the positions of the left track and the right track output by the track prediction model at all moments in the current prediction period;

fitting the positions of the left crawler belt at all the moments in the current prediction period to obtain a prediction track of the left crawler belt in the current prediction period, and fitting the positions of the right crawler belt at all the moments in the current prediction period to obtain a prediction track of the right crawler belt in the current prediction period.

9. An electronic device, the electronic device comprising:

a processor and a memory;

the processor is configured to execute the steps of the tracked vehicle trajectory prediction method according to any one of claims 1 to 8 by calling a program or instructions stored in the memory.

10. A computer-readable storage medium storing a program or instructions that cause a computer to perform the steps of the tracked vehicle track prediction method according to any one of claims 1 to 8.