CN114384902A

CN114384902A - Automatic tracking control method and system thereof

Info

Publication number: CN114384902A
Application number: CN202011116729.7A
Authority: CN
Inventors: 张新锐; 袁希文; 张沙; 黄瑞鹏; 张乾; 胡云卿
Original assignee: CRRC Zhuzhou Institute Co Ltd
Current assignee: CRRC Zhuzhou Institute Co Ltd
Priority date: 2020-10-19
Filing date: 2020-10-19
Publication date: 2022-04-22
Anticipated expiration: 2040-10-19
Also published as: CN114384902B

Abstract

The invention provides an automatic tracking control method, which is suitable for a vehicle with an automatic tracking function, wherein the vehicle automatically tracks a virtual track corresponding to a driving route of the vehicle to run in a tracking state, and a plurality of navigation path points are arranged on the virtual track corresponding to the driving route of the vehicle, and the automatic tracking control method comprises the following steps: determining a nearest navigation waypoint passed by the vehicle based on a current driving state of the vehicle; determining a current preview point of the vehicle based on the nearest navigation path point and a current preview distance; and utilizing a pure tracking control algorithm to determine a steering control strategy for the vehicle to track the current home point.

Description

Automatic tracking control method and system thereof

Technical Field

The invention relates to the field of vehicle steering control, in particular to a method and a system suitable for automatic tracking control.

Background

The electric car is a common public transport passenger car, and comprises a rail electric car, a light rail electric car, a tramcar and the like. The existing rail electric cars, light rail electric cars and tramcars need a special electric power system and a rail to cooperate to realize operation, and the infrastructure construction and vehicle acquisition cost are high. In order to solve the problem, the middle school bus group provides an electric bus concept capable of tracking the virtual track on the ground, and the novel electric bus cancels a steel rail and runs along the virtual track on the ground in a mode of rubber wheel bearing and steering of a steering wheel.

The virtual track on the ground can be flexibly arranged, special capital construction does not need to be carried out on the ground, and the virtual track for driving the novel trolley bus only needs to be drawn on the ground like a lane line and a zebra crossing. This kind of novel trolley-bus need not to travel along the fixed track again, and greatly reduced capital construction cost, has huge operation advantage for the tram. Meanwhile, the novel electric car has the running characteristics of road right sharing and mixed traffic, so that the traffic system has the advantage of flexible organization in the aspects of ground lane arrangement and the like.

In 2016, the electric car developed by the Taoise achieves the automatic tracking function based on vision, and the intelligent level of the novel electric car is further improved. From the release of an intelligent express system in 6 months in 2017 to the opening of a T1 line of an intelligent express system in Sichuan Yibin in 6 months in 2019, the novel electric car is opened and operated in three cities of Hunan, Tazhou, Jiangxi Yongyun, Sichuan Yibin and the like in succession, the requirement of a customer on a tracking function is gradually improved, and the requirement on the safety and reliability of the tracking system is higher and higher from initial tracking auxiliary entry to current full-line full-time tracking driving.

The automatic tracking function becomes a vital function in the automatic driving or the auxiliary driving of the novel electric car. In the field of vehicle automatic driving, a transverse control theory based on navigation has been deeply researched by predecessors, and a paradigm of realizing a control effect by successfully loading is provided.

The pure tracking control algorithm is widely applied to path tracking control of the unmanned vehicle. When the algorithm is used for tracking a straight line section, the tracking speed is high, the precision is high, and the algorithm is matched with the running scene of the novel electric car. The pure tracking control algorithm requires that the expected track is known, and the expected track can be obtained by actually tracking the navigation path points on the preset virtual track, and can also be obtained by using a piecewise fitting curve of the navigation path points on the preset virtual track. To improve the stability of the pure tracking control, the density of navigation path points on the virtual trajectory may be increased or the fitted curve may be ensured to be continuous everywhere, but the consumption of computing resources may be increased accordingly.

The existing processors equipped on the novel vehicles have low processing capacity and are difficult to perform real-time calculation and real-time curve fitting of a large number of path points in a short time. In order to solve the problem of low processing capacity of the existing processor, the invention aims to provide an automatic tracking control method which can improve the existing pure tracking control algorithm, reduce the consumption of processing operation resources, improve the operation efficiency and realize better tracking control performance.

Disclosure of Invention

The following presents a simplified summary of one or more aspects in order to provide a basic understanding of such aspects. This summary is not an extensive overview of all contemplated aspects, and is intended to neither identify key or critical elements of all aspects nor delineate the scope of any or all aspects. Its sole purpose is to present some concepts of one or more aspects in a simplified form as a prelude to the more detailed description that is presented later.

According to an aspect of the present invention, there is provided an automatic tracking control method for a vehicle having an automatic tracking function, the vehicle automatically tracks a virtual track corresponding to a driving route of the vehicle in a tracking state, and a plurality of navigation waypoints are arranged on the virtual track corresponding to the driving route of the vehicle, the automatic tracking control method including: determining a nearest navigation waypoint passed by the vehicle based on a current driving state of the vehicle; determining a current preview point of the vehicle based on the nearest navigation path point and a current preview distance; and utilizing a pure tracking control algorithm to determine a steering control strategy for the vehicle to track the current home point.

In one embodiment, the current driving state includes a current position and a current heading of the vehicle, each navigation waypoint on the driving route has a pre-collected preset heading, and the determining a nearest navigation waypoint passed by the vehicle based on the current driving state of the vehicle includes: responding to the vehicle in a searching state, and determining the nearest navigation path point based on the distance between each navigation path point and the current position and the course included angle of the navigation path point; and responding to the fact that the vehicle is in a search-free state, and judging whether to update the next navigation path point to be the nearest navigation path point or not based on the vector relation from the current nearest navigation path point to the next navigation path point and the mass center of the vehicle, wherein the current nearest navigation path point is the nearest navigation path point determined in the previous judging period.

In an embodiment, the determining the nearest navigation waypoint based on the distance from each navigation waypoint to the current position and the heading angle thereof includes: calculating the distance from each navigation path point to the current position one by one along the driving route and judging whether the navigation path point is a temporary nearest navigation path point in the currently calculated navigation path points; and determining a most recently determined temporary latest navigation waypoint as the latest navigation waypoint until a final navigation waypoint to the driving route is calculated.

In one embodiment, the distance calculation and determination process for each navigation path point includes: judging whether the distance from a current waypoint to the current position is smaller than the current nearest distance, wherein the current waypoint is a navigation waypoint of which the distance is being calculated, and the current nearest distance is the distance from a temporary nearest navigation waypoint determined before the current waypoint to the current position; responding to the fact that the distance between the current waypoint and the current position is smaller than the current nearest distance, and judging whether a course included angle between the course of the current waypoint and the current course of the vehicle is smaller than 90 degrees or not; in response to that the distance from the current waypoint to the current position is greater than or equal to the current nearest distance or the included angle between the course of the current waypoint and the current course of the vehicle is greater than or equal to 90 degrees, updating the next navigation waypoint of the current waypoint into the current waypoint in the next calculation and judgment process, and maintaining the temporary nearest navigation waypoint and the current nearest distance unchanged; and responding to the fact that the included angle between the current course of the current waypoint and the current course of the vehicle is smaller than 90 degrees, updating the current waypoint to be the temporary nearest navigation path point, updating the distance between the current waypoint and the current position to be the current nearest distance, and updating the next navigation path point of the current waypoint to be the current waypoint in the next calculation and judgment process.

In one embodiment, the determining whether to update the next navigation waypoint to the closest navigation waypoint based on the vector relationship from the current closest navigation waypoint to the next navigation waypoint and the centroid of the vehicle includes: taking a vector formed by the current navigation path point pointing to the center of mass of the vehicle as a vehicle vector; taking a vector formed by the current navigation path point pointing to the next navigation path point as a route vector; calculating a judgment variable of the vehicle vector and the route vector

Wherein the coordinates of the center of mass of the vehicle is N (x)_n,y_n) The coordinate of the current nearest navigation path point is P (x)_k,y_k) The coordinate of the next navigation path point of the nearest navigation path point is Q (x)_k+1,y_k+1) (ii) a Responding to the judgment variable value smaller than 1, and maintaining the current nearest navigation path point as the nearest navigation path point; and updating the next navigation path point to the nearest navigation path point in response to the judgment variable value being greater than or equal to 1.

In one embodiment, the determining a current preview point of the vehicle based on the closest navigation path point and a current preview distance comprises: starting from the nearest navigation path point, taking each navigation path point which is positioned on the driving route and behind the nearest navigation path point as a current navigation path point one by one, and judging whether the pre-aiming arc length from the current navigation path point to the nearest navigation path point is smaller than the pre-aiming distance and whether the current navigation path point is the final navigation path point of the driving route until the pre-aiming arc length from the current navigation path point to the nearest navigation path point is larger than or equal to the pre-aiming distance or the current navigation path point is the final navigation path point of the driving route; in response to the fact that the pre-aiming arc length from the current navigation path point to the nearest navigation path point at the end is larger than or equal to the current pre-aiming distance, determining a point which is located between the current navigation path point and the previous navigation path point and has the pre-aiming arc length with the nearest navigation path point equal to the current pre-aiming distance by adopting a linear interpolation method, and determining the point as the current pre-aiming point; and determining a final navigation path point of the driving route as the current preview point in response to the preview arc length of the current navigation path point at the end being less than the preview distance.

In an embodiment, the navigation path points on the driving route are arranged at equal intervals, the distance between two adjacent navigation path points is a preset arc length, the navigation path points on the driving route are numbered sequentially, and the process of determining whether the pre-aiming arc length from the current navigation path point to the nearest navigation path point is less than the pre-aiming distance and whether the current navigation path point is the final navigation path point of the driving route each time includes: multiplying the difference between the number of the current navigation path point and the number of the nearest navigation path point by the preset arc length to obtain the pre-aiming arc length from the current navigation path point to the nearest navigation path point; judging whether the pre-aiming arc length from the current navigation path point to the nearest navigation path point is smaller than the current pre-aiming distance and whether the number of the current navigation path point is smaller than the number of the final navigation path point of the driving route; adding 1 to the number of the current navigation path point in response to that the pre-aiming arc length from the current navigation path point to the nearest navigation path point is less than the current pre-aiming distance and the number of the navigation path point is less than the number of the final navigation path point of the driving route; and responding to the fact that the pre-aiming arc length from the current navigation path point to the nearest navigation path point is larger than or equal to the current pre-aiming distance or the number of the navigation path point is equal to the number of the final navigation path point of the driving route, and finishing the judgment.

In one embodiment, the automatic tracking control method further comprises: calculating the current pre-aiming distance based on the current driving speed of the vehicle and the curvature of the nearest navigation path point.

In one embodiment, the calculating the current pre-address distance based on the current driving speed of the vehicle and the curvature of the nearest navigation path point comprises: using formula of calculation of pre-aiming distance

Determining the current pre-aiming distance corresponding to the nearest navigation path point, wherein l_dIs the current pre-aiming distance, v is the current running speed of the vehicle, ρ is the curvature of the nearest navigation path point, c₁、c₂And c₃Are each a constant.

According to another aspect of the present invention, there is also provided an automatic tracking control apparatus, comprising a memory, a processor and a computer program stored on the memory, the processor being adapted to implement the steps of the automatic tracking control method according to any one of the above when executing the computer program stored on the memory.

According to yet another aspect of the present invention, there is also provided a computer storage medium having a computer program stored thereon, the computer program when executed implementing the steps of the automatic tracking control method according to any one of the above.

According to another aspect of the present invention, there is also provided an automatic tracking control system, adapted to a vehicle having an automatic tracking function, the vehicle automatically tracks a virtual track corresponding to a driving route of the vehicle in a tracking state, a plurality of navigation path points are arranged on the virtual track corresponding to the driving route of the vehicle, the automatic tracking control system includes an inertial satellite integrated navigation unit, a vehicle body sensor, an automatic tracking control unit and a steering execution unit, the automatic tracking control unit is respectively connected to the inertial satellite integrated navigation unit, the vehicle body sensor and the steering execution unit, the inertial satellite integrated navigation unit is used for determining a current driving state of the vehicle, the vehicle body sensor is used for providing auxiliary state information to the automatic tracking control unit, and the automatic tracking control unit is used for implementing the steps of the automatic tracking control method as described in any one of the above to determine the vehicle The steering execution unit is used for executing the steering control strategy.

Drawings

The above features and advantages of the present disclosure will be better understood upon reading the detailed description of embodiments of the disclosure in conjunction with the following drawings.

FIG. 1 is a schematic illustration of a driving route in a hypothetical scene depicted in accordance with one aspect of the present invention;

FIG. 2 is a flow chart illustrating an exemplary method of automatic tracking control according to an aspect of the present invention;

FIG. 3 is a partial flow diagram of an exemplary automatic tracking control method according to an aspect of the present invention;

FIG. 4 is a partial flow diagram of an exemplary automatic tracking control method according to an aspect of the present invention;

5A-5C are schematic views of the projected positions of vehicle vectors under different conditions according to an aspect of the present invention;

FIG. 6 is a partial flow diagram of an exemplary automatic tracking control method according to an aspect of the present invention;

FIG. 7 is a partial flow diagram of an exemplary automatic tracking control method according to an aspect of the present invention;

FIG. 8 is a partial flow diagram of an exemplary automatic tracking control method according to an aspect of the present invention;

FIGS. 9A-9C are schematic diagrams illustrating the relationship between a current navigation waypoint and a home address point location for various situations in accordance with an aspect of the disclosure;

FIG. 10 is a block diagram of an automatic tracking control apparatus in an embodiment according to another aspect of the present invention;

FIG. 11 is a block diagram of modules of an automatic tracking control system in an embodiment according to yet another aspect of the invention;

FIG. 12 is a block diagram of the modules of an automatic tracking control system in an embodiment according to yet another aspect of the invention.

Detailed Description

The following description is presented to enable any person skilled in the art to make and use the invention and is incorporated in the context of a particular application. Various modifications, as well as various uses in different applications will be readily apparent to those skilled in the art, and the generic principles defined herein may be applied to a wide range of embodiments. Thus, the present invention is not intended to be limited to the embodiments shown herein but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

In the following detailed description, numerous specific details are set forth in order to provide a more thorough understanding of the invention. It will be apparent, however, to one skilled in the art that the practice of the invention may not necessarily be limited to these specific details. In other instances, well-known structures and devices are shown in block diagram form, rather than in detail, in order to avoid obscuring the present invention.

The reader's attention is directed to all papers and documents which are filed concurrently with this specification and which are open to public inspection with this specification, and the contents of all such papers and documents are incorporated herein by reference. All the features disclosed in this specification (including any accompanying claims, abstract and drawings) may be replaced by alternative features serving the same, equivalent or similar purpose, unless expressly stated otherwise. Thus, unless expressly stated otherwise, each feature disclosed is one example only of a generic series of equivalent or similar features.

Note that where used, the designations left, right, front, back, top, bottom, positive, negative, clockwise, and counterclockwise are used for convenience only and do not imply any particular fixed orientation. In fact, they are used to reflect the relative position and/or orientation between the various parts of the object. Furthermore, the terms "first" and "second" are used for descriptive purposes only and are not to be construed as indicating or implying relative importance.

In the description of the present invention, it should be noted that, unless otherwise explicitly specified or limited, the terms "mounted," "connected," and "connected" are to be construed broadly, e.g., as meaning either a fixed connection, a removable connection, or an integral connection; can be mechanically or electrically connected; they may be connected directly or indirectly through intervening media, or they may be interconnected between two elements. The specific meanings of the above terms in the present invention can be understood in specific cases to those skilled in the art.

It is noted that, where used, further, preferably, still further and more preferably is a brief introduction to the exposition of the alternative embodiment on the basis of the preceding embodiment, the contents of the further, preferably, still further or more preferably back band being combined with the preceding embodiment as a complete constituent of the alternative embodiment. Several further, preferred, still further or more preferred arrangements of the belt after the same embodiment may be combined in any combination to form a further embodiment.

The invention is described in detail below with reference to the figures and specific embodiments. It is noted that the aspects described below in connection with the figures and the specific embodiments are only exemplary and should not be construed as imposing any limitation on the scope of the present invention.

According to an aspect of the present invention, an automatic tracking control method is provided for a vehicle having an automatic tracking function. As shown in fig. 1, assuming that virtual tracks T1 and T2 are drawn for a tracked vehicle on a road, a plurality of navigation path points D may be further arranged on a virtual track T1 corresponding to a driving route of the vehicle. It is understood that the virtual tracks T1 or T2 are lines or line segments (illustrated in fig. 1 as simple lines) having no direction, and a segment of the virtual track may be shared by vehicles having different driving routes. The driving route is a route with a direction formed by combining a plurality of sections of virtual tracks, and the vehicle of the driving route drives along the virtual track corresponding to the driving route.

In one embodiment, as shown in FIG. 2, the automatic tracking control method 200 includes steps S210-S230.

Wherein, step S210 is: and determining the nearest navigation path point passed by the vehicle based on the current running state of the vehicle.

As shown in fig. 1, it is assumed that the virtual track corresponding to the driving route of the vehicle a is T1, and the current position of the vehicle a is the navigation waypoint D_iAnd D_i+1In between, the nearest navigation route point that the vehicle A passes is the navigation route point D_i。

The current running state is used to indicate the driving condition of the vehicle, and includes at least state data indicating the position of the vehicle on the running route and state data indicating the driving direction.

In the prior art, a conventional method for determining a navigation path point closest to a vehicle on a driving route is to calculate distances from all navigation path points on the driving route to the vehicle in each traversal and determine a navigation path point with the smallest distance as the closest navigation path point. Although the conventional method is simple in principle, the square sum and the square sum are required to be performed for many times, the calculation process is complicated, the calculation amount is large, and the time consumption is long. To solve the problems of the conventional method, the present invention divides the vehicle into a search state and a search-free state, corresponding to the case where the current nearest navigation waypoint of the vehicle is unknown and known, respectively.

When the current nearest navigation path point of the vehicle is unknown, the nearest navigation path point of the vehicle is calculated by adopting the conventional method. That is, in response to the vehicle being in the search state, the nearest navigation waypoint may be determined based on the distance from each navigation waypoint to the current position of the vehicle and the heading angle thereof.

It is understood that the heading of the vehicle may indicate the direction in which the driving route extends, and the current position of the vehicle may be used to determine the distance between each navigation waypoint and the vehicle.

Because the navigation path points are arranged along the driving route, a test vehicle can be used for carrying out a standard driving test along the driving route, and the heading of each navigation path point is collected by a gyroscope or other direction collecting equipment on the test vehicle at each navigation path point in the test process.

It can be understood that, since the vehicle needs to travel along the driving route, and the heading of the nearest navigation waypoint of the vehicle is almost the same as the extending direction of the driving route, the heading angle between the heading of the vehicle and the heading of the nearest navigation waypoint of the vehicle should not exceed 90 °, and if the heading angle between a certain navigation waypoint and the heading of the vehicle exceeds 90 °, the navigation waypoint is not the nearest navigation waypoint.

Specifically, as shown in fig. 3, the navigation path point in the search state may be determined through steps S310 to S320.

Wherein, step S310 is: and calculating the distance from each navigation path point to the current position of the vehicle one by one along the driving route of the vehicle and judging whether each navigation path point is a temporary nearest navigation path point in the currently calculated navigation path points.

Step S320 is: and determining a temporary nearest navigation path point in the currently calculated navigation path points as the nearest navigation path point until a final navigation path point to the driving route is calculated.

Suppose a vehicle includes navigation waypoints D on its route₁～D_NThe navigation path points D₁～D_NThe running routes are arranged in sequence according to the numbers. Then D is firstly added₁Determining as a temporary nearest navigation path point; then D is judged₁And D₂The temporary closest navigation waypoint in (1); then D is judged₁～D₃The temporary nearest navigation path point in the navigation system is judged to be D₁～D₄Until D is determined₁～D_NThe determined temporary nearest navigation route point is the actual nearest navigation route point of the current position of the vehicle.

Specifically, with any navigation waypoint D_i(1 < i < N) as an example to illustrate distance calculation for each navigation route pointAnd a judgment process. Navigation waypoint D_iPreviously determined D₁～D_i-1The temporary nearest navigation waypoint in (1) is D_T(T is more than or equal to 1 and less than or equal to i-1), and the current closest distance is a temporary closest navigation path point D_TDistance L to the current position of the vehicle_T. Then judge D₁～D_iThe process of navigating the waypoints temporarily closest in (1) may include steps S311 to S314 as shown in fig. 4.

Wherein, step S311 is: judging whether the distance from the current waypoint to the current position is smaller than the current nearest distance, wherein the current waypoint is the navigation waypoint of which the distance is being calculated, and the current nearest distance is the distance from the temporary nearest navigation waypoint determined before the current waypoint to the current position.

Then at navigation waypoint D_iIn the distance calculating and determining process of (3), step S311 is: judging navigation path point D_iWhether the distance to the current position of the vehicle is less than the current closest distance L_T。

If the navigation path point D_iThe distance to the current position of the vehicle is less than the current closest distance L_TStep S312 is executed, if the navigation route point D is located_iThe distance to the current position of the vehicle is greater than or equal to the current closest distance L_TStep S313 is executed.

Step S312 is: and in response to the fact that the distance between the current waypoint and the current position is smaller than the current nearest distance, judging whether a course included angle between the course of the current waypoint and the current course of the vehicle is smaller than 90 degrees.

The current course of the vehicle is the course which is acquired by the inertial satellite integrated navigation unit on the vehicle in real time when the vehicle is at the current position. The course of the current waypoint is the course which is acquired by an inertial satellite combined navigation unit arranged on the test vehicle at the position of the current waypoint in the process of the pre-acquisition test driving of the test vehicle at the navigation waypoint. And the included angle between the current course of the current waypoint and the current course of the vehicle is the included angle of the two courses in the same coordinate system.

It will be appreciated that a heading angle of less than 90 ° between the current heading of the current waypoint and the current heading of the vehicle indicates that the vehicle is still traveling along its travel route, has not deviated from the travel route and is not in the opposite travel direction to the travel route.

At navigation waypoint D_iIn the distance calculating and determining process of (3), step S312 is: judging navigation path point D_iIs less than 90 deg. from the heading of the vehicle at its current location. If the navigation path point D_iIs less than 90 degrees with respect to the heading of the vehicle at its current position, step S314 is performed; if the navigation path point D_iIs greater than or equal to 90 deg., and the heading of the vehicle at its current position, step S313 is performed.

Step S313 is: and in response to the fact that the distance between the current waypoint and the current position is greater than or equal to the current nearest distance or the included angle between the heading of the current waypoint and the current heading of the vehicle is greater than or equal to 90 degrees, updating the next navigation waypoint of the current waypoint into the current waypoint in the next calculation and judgment process, and maintaining the temporary nearest navigation waypoint and the current nearest distance unchanged.

I.e. at navigation waypoint D_iWhen the navigation path point D is in the process of calculating and judging the distance_iThe distance to the current position of the vehicle is greater than or equal to the current closest distance L_TOr navigation waypoint D_iWhen the included angle between the course of the vehicle and the course of the vehicle at the current position of the vehicle is more than or equal to 90 degrees, the step D is carried out_i+1Updating the current waypoint to be calculated and judged next time and maintaining the navigation waypoint D_TFor the temporary nearest navigation waypoint and navigation waypoint D_TDistance L to the current position of the vehicle_TThe current closest distance is unchanged.

Step S314 is: and in response to the fact that the included angle between the current course of the current waypoint and the current course of the vehicle is smaller than 90 degrees, updating the current waypoint into a temporary nearest navigation waypoint, updating the distance between the current waypoint and the current position of the vehicle into a current nearest distance, and updating the next navigation waypoint of the current waypoint into the current waypoint in the next calculation and judgment process.

I.e. at navigation waypoint D_iWhen the navigation path point D is in the process of calculating and judging the distance_iThe distance to the current position of the vehicle is less than the current closest distance L_TAnd navigate waypoint D_iWhen the included angle between the course of the vehicle and the course of the vehicle at the current position of the vehicle is less than 90 degrees, D is calculated_iIs updated to D₁～D_iTemporary closest navigation waypoints of (1), will D_iDistance L to the current position of the vehicle_iIs updated to D₁～D_iAnd D is the current closest distance in (1)_i+1And updating the route points to the current route points calculated and judged next time.

In particular, in the calculation and determination process of the first navigation waypoint of the driving route, since only one navigation waypoint exists, the first navigation waypoint is directly updated to the temporary closest navigation waypoint, the distance from the first navigation waypoint to the current position of the vehicle is updated to the current closest distance for the next calculation and determination, and the second navigation waypoint is updated to the current waypoint for the next calculation and determination.

Particularly, in the process of calculating and judging the last navigation path point of the driving route, if the distance from the last navigation path point to the current position of the vehicle is less than the current nearest distance and the included angle between the heading of the last navigation path point and the current heading of the vehicle is less than 90 degrees, the last navigation path point is updated to be the nearest navigation path point of the vehicle, and the last navigation path point is ended, otherwise, the temporary nearest navigation path point obtained in the previous calculation and judgment process, namely the latest determined temporary nearest navigation path point is determined to be the nearest navigation path point of the vehicle.

Correspondingly, when the current latest navigation route point of the vehicle is known, the vehicle is in a search-free state, and whether the next navigation route point needs to be updated to the latest navigation route point can be judged based on the vector from the known current latest navigation route point to the next navigation route point and the vector from the current latest navigation route point to the center of mass of the vehicle. Namely, the determined nearest navigation path point is taken as a reference to judge whether the nearest navigation path point needs to be updated or not.

Specifically, as shown in FIGS. 5A to 5C,suppose the centroid coordinate of the vehicle is N (x)_n,y_n) Point, P (x)_k,y_k) And Q (x)_k+1,y_k+1) The k navigation path point and the k +1 navigation path point on the driving route are respectively, the point S is a projection point of the point N on a straight line where the PQ is located, and a line segment formed by connecting the point N and the PQ has three position relations.

(Vector)

Is composed of

The projection on the line PQ has directivity. Defining a criterion for determining

Variable R in relation to line segment PQ_nThe variable R_nThe calculation formula of (2) is as follows:

as can be seen from FIG. 5, when R is_nIf < 0, it is the case of (1); when 0 < R_nIf < 1, it is the case of (2); when R is_nIf the value is more than 1, the case is the case (3); when R is_nWhen the value is 0, the S point coincides with the P point, which is the case of (2); when R is_nWhen the value is 1, the S point coincides with the Q point, which is the case of (2).

The point P is assumed to be a known nearest navigation route point of the vehicle, and may specifically be a nearest navigation route point determined in the aforementioned search state or a nearest navigation route point determined in the last vector determination process. When R is_nWhen the navigation path point P is not less than 1, the train crosses the navigation path point P, and the navigation path point Q next to the point P can be updated to the latest navigation path point.

Suppose a vehicle includes navigation waypoints D on its route₁～D_NThe navigation path points D₁～D_NArranged in sequence according to the number along the driving route, and then arranged with any nearestNavigation waypoint D_iThe process of updating the latest navigation route point in the search-free state will be described as an example (1 < i < N).

Specifically, as shown in fig. 6, the updating process of any nearest navigation waypoint in the search-free state may include steps S610 to S650.

Wherein, S610 is: and taking a vector formed by the current navigation path point pointing to the center of mass of the vehicle as a vehicle vector.

Taking any of FIGS. 5A-5C as an example, assume the nearest navigation waypoint D_iHas the coordinate of P (x)_k,y_k) The coordinate of the center of mass of the vehicle is N (x)_n,y_n) And the vehicle vector is a directed line segment PN.

S620 is as follows: and taking a vector formed by the current navigation path point pointing to the next navigation path point as a route vector.

Taking any of FIGS. 5A-5C as an example, assume the nearest navigation waypoint D_iHas the coordinate of P (x)_k,y_k) Nearest navigation waypoint D_iThe coordinate of the next navigation path point of (2) is Q (x)_k+1,y_k+1) Then the route vector is a directed line segment PQ.

S630 is: and calculating judgment variables of the vehicle vector and the route vector. Coordinate N (x) of the center of mass of the vehicle_n,y_n) Coordinate P (x) of current nearest navigation path point_k,y_k) And the coordinates Q (x) of the next navigation path point_k+1,y_k+1) Substitution of formula (1) to obtain a judgment variable R_nThe value of (c).

Taking any one of fig. 5A to 5C as an example, assuming that a projection point of the centroid of the vehicle onto a straight line where the line segment PQ is located is S, the projection vector is an oriented line segment PS, and it can be understood that when the oriented line segment PS is the same as the direction of the route vector, (x)_k+1-x_k)·(x_n-x_k)+(y_k+1-y_k)·(y_n-y_k) Is positive, R_nIf the length of the line segment PS is greater than or equal to the modulus of the route vector, R is greater than 0_nNot less than 1, corresponding to the situation shown in FIG. 5C, if the length of the line segment PS is smaller than the modulus of the route vector, then R is_n< 1, which may correspond to the situation shown in FIG. 5B; if there isThe direction of the line segment PS is opposite to the direction of the course vector, (x)_k+1-x_k)·(x_n-x_k)+(y_k+1-y_k)·(y_n-y_k) Is negative, R_n< 0, which may correspond to the situation shown in FIG. 5A.

Step S640 is: and responding to the judgment variable with the value smaller than 1, and maintaining the current nearest navigation path point as the nearest navigation path point.

To determine whether to update the current latest navigation path point D_iFor example, if the value of the determination variable is less than 1, the current nearest navigation waypoint D is maintained_iThe waypoints are unchanged for the most recent navigation.

Step S650 is: and updating the next navigation path point to the nearest navigation path point in response to the judgment variable value being greater than or equal to 1.

To determine whether to update the current latest navigation path point D_iFor example, if the value of the determination variable is greater than or equal to 1, the current nearest navigation waypoint D_iNext navigation path point D_i+1Updating to the latest navigation path point and using the navigation path point D in the next updating judgment process_i+1The current nearest navigation path point.

By combining the searching state and the search-free state of the vehicle, the invention can determine the nearest navigation path point by the determining step of the nearest navigation path point in the searching state for the first time, then subsequently judge whether the vehicle crosses the current nearest navigation path point by adopting the vector projection method in the search-free state, and if the vehicle crosses the current nearest navigation path point, update the nearest navigation path point. The combination does not need to perform evolution and squaring for many times, the calculation amount is small, the program implementation efficiency is high, the consumption of processing and calculation resources is obviously reduced, the calculation of more navigation path points can be supported under the condition of limited consumption of the processing and calculation resources, the higher density of the navigation path points can be supported in the same section of driving route, and the control effect of the pure tracking control algorithm is better.

Further, after determining the nearest navigation route point of the vehicle, step S220 is: and determining the current preview point of the vehicle based on the nearest navigation path point and the current preview distance.

The target point is a tracking target of a pure tracking algorithm, and the pure tracking algorithm tracks the corresponding target point at different positions to determine the steering control strategy. The current preview point refers to the nearest navigation path point corresponding to the current position of the vehicle and the preview point associated with the current preview distance.

The preview distance is mainly used for determining a preview point according to the curvature change of the driving route where the vehicle is located and all changes of the current driving speed, and generally, a point on the driving route and having a distance from the nearest navigation route point as the preview distance is the preview point, with the nearest navigation route point as a starting point.

Since the navigation waypoints are the navigation assistance marks arranged in advance on the driving route, the distance between two adjacent navigation waypoints on the driving route is designed in advance, that is, the distance between any two adjacent navigation waypoints is known or is equal. Therefore, the pre-aiming arc length between the nearest navigation path point and any one navigation path point after the nearest navigation path point can be determined based on the preset arc length from the nearest navigation path point to all navigation path points in the navigation path points, and the road section where the pre-aiming point is located can be roughly determined based on the magnitude relation between the pre-aiming arc length and the pre-aiming distance.

The arc length is the length of an arc formed by the sections of the driving route between any two navigation path points when the driving route passes through the two navigation path points respectively. The preset arc length is a known arc length between two adjacent navigation path points, and the preset arc length can be a fixed arc length when the navigation path points on the driving route are equidistantly arranged. When judging whether the preview point is between the nearest navigation path point and another navigation path point, the preview arc length refers to the length of an arc formed by a road section of the driving route between the nearest navigation path point and the another navigation path point.

Specifically, as shown in FIG. 7, step S220 may include steps S221-S223.

Wherein, step S221 is: and starting from the nearest navigation path point, taking each navigation path point which is positioned on the driving route and behind the nearest navigation path point as the current navigation path point one by one, and judging whether the pre-aiming arc length from the current navigation path point to the nearest navigation path point is less than the pre-aiming distance and whether the current navigation path point is the final navigation path point of the driving route until the pre-aiming arc length from the current navigation path point to the nearest navigation path point is more than or equal to the pre-aiming distance or the current navigation path point is the final navigation path point of the driving route.

Suppose a vehicle includes navigation waypoints D on its route₁～D_NThe navigation path points D₁～D_NThe numbers of the navigation path points D are 1 to N in sequence₁～D_NArranged in sequence according to the number along the driving route, and navigating route points D_i(i is more than or equal to 1 and less than or equal to N) as the nearest navigation path point, step S221 determines D first_i+1Distance to nearest navigation path point D_iWhether the pre-aiming arc length is less than the pre-aiming distance and D_i+1Whether it is the final navigation path point of the driving route, if D_i+1Distance to nearest navigation path point D_iThe pre-aiming arc length is greater than or equal to the pre-aiming distance or D_i+1If the navigation route is the final navigation route point of the driving route, finishing the judgment, and if the navigation route is the final navigation route point of the driving route, finishing the judgment_i+1Distance to nearest navigation path point D_iThe pre-aiming arc length is less than the pre-aiming distance and D_i+1If the navigation route is not the final navigation route point of the driving route, D is continuously judged_i+2Distance to nearest navigation path point D_iWhether the pre-aiming arc length is less than the pre-aiming distance and D_i+2Whether the navigation path point is the final navigation path point of the driving route; and so on until the end condition is satisfied.

The process of determining whether the preview arc length from any navigation path point located after the nearest navigation path point to the nearest navigation path point is less than the preview distance and whether the navigation path point is the final navigation path point of the driving route may be as shown in fig. 8, including steps S810 to S840.

Wherein, step S810 is: and multiplying the difference between the number of the current navigation path point and the number of the nearest navigation path point by a preset arc length to obtain the pre-aiming arc length from the current navigation path point to the nearest navigation path point.

To navigate waypoint D_iAs the nearest navigation waypoint D_iTo be located atNearest navigation waypoint D_iAny navigation path point D_jFor example, the determination process of (i is not greater than j is not greater than N), and step S810 may specifically correspond to: the nearest navigation path point D_iAnd navigation waypoint D_jThe serial number of the navigation path point D is substituted into a pre-aiming arc length calculation formula (2) to calculate the navigation path point D_jAnd the nearest navigation waypoint D_iThe preview arc length in between.

l_ij＝(j-i)×l₀ (2)

Wherein l_ijFor navigating the waypoint D_jAnd the nearest navigation waypoint D_iThe pre-aiming arc length between, i is the nearest navigation path point D_iJ is a navigation path point D_jNumber of (1)₀Is a preset arc length.

In order to solve the current preview point, the conventional method is to calculate the accumulated arc length from the nearest navigation path point to the traversed current navigation path point, and when the accumulated arc length is just greater than or equal to the preview distance, the traversed current navigation path point is taken as the final current navigation path point. By arranging the equally spaced waypoints, the method only needs to use the formula (2) to calculate when calculating the preview arc length. The calculation amount is small, the code implementation efficiency is high, the consumption of processing operation resources is obviously reduced, the calculation of more navigation path points can be supported under the condition of limited consumption of the processing operation resources, the higher density of the navigation path points can be supported in the same section of driving route, and the control effect of the pure tracking control algorithm is better.

Step S820 is: and judging whether the pre-aiming arc length from the current navigation path point to the nearest navigation path point is less than the current pre-aiming distance and whether the number of the current navigation path point is less than the number of the final navigation path point of the driving route.

To navigate waypoint D_iAs the nearest navigation waypoint D_iTo be located at the nearest navigation path point D_iAny navigation path point D_j(j is not less than i and not more than N) as an example, assuming that the current pre-aiming distance is L, thenStep S820 may specifically correspond to: judging the pre-aiming arc length l_ijWhether the current pre-aiming distance L is less than the current pre-aiming distance, and whether j is less than N, if the pre-aiming arc length is L_ijIf the current pre-aiming distance L is less than the j and j is less than N, executing the step S830, if the pre-aiming arc length L is less than the N_ijIf the current preview distance is greater than or equal to L or j is equal to N, the step S840 is executed.

Step S830 is: and in response to that the pre-aiming arc length from the current navigation path point to the nearest navigation path point is less than the current pre-aiming distance and the number of the navigation path point is less than the number of the final navigation path point of the driving route, adding 1 to the number of the current navigation path point.

To navigate waypoint D_iAs the nearest navigation waypoint D_iTo be located at the nearest navigation path point D_iAny navigation path point D_j(i is not more than j is not more than N), the step S830 may specifically correspond to: will be determined as D_j+1Determined as the current leading waypoint to make the next turn D_j+1The judgment process of (1).

Step S840 is: and responding to the fact that the pre-aiming arc length from the current navigation path point to the nearest navigation path point is larger than or equal to the current pre-aiming distance or the number of the navigation path point is equal to the number of the final navigation path point of the driving route, and finishing the judgment.

To navigate waypoint D_iAs the nearest navigation waypoint D_iTo be located at the nearest navigation path point D_iAny navigation path point D_j(i is not greater than j is not greater than N), for example, the step S840 may specifically correspond to: maintaining the current navigation path point as D_j。

It can be understood that there are two situations in the latest current navigation path point determined in step S221, one is that the preview arc length between the current navigation path point and the latest navigation path point is greater than or equal to the current preview distance, and the other is that the current navigation path point is the final navigation path point. However, there may be a current navigation path point that satisfies the two situations, and for the current navigation path point that satisfies the two situations, the current preview point should be located between the final navigation path point and the previous navigation path point, so the current preview point needs to be determined based on the relationship between the preview arc length and the preview distance between the current navigation path point and the nearest navigation path point.

Accordingly, step S222 is: and in response to the fact that the pre-aiming arc length from the current navigation path point to the nearest navigation path point is larger than or equal to the current pre-aiming distance, determining a point which is located between the current navigation path point and the navigation path point before the current navigation path point and has the pre-aiming arc length equal to the current pre-aiming distance with the nearest navigation path point by adopting a linear interpolation method, and determining the point as the current pre-aiming point.

That is, assume that the nearest navigation waypoint is D_iWhen step S221 ends, the current navigation waypoint is D_j(i < j < N), if the current navigation path point is D_jAnd the nearest navigation path point is D_iPre-aiming arc length l between_ijGreater than or equal to the current preview distance L, the current preview point O should be located at D as shown in FIG. 9A or 9B_j-1And D_jOn the driving route therebetween. Fig. 9A corresponds to a case where j < N, and fig. 9B corresponds to a case where j is N.

Step S223 is: and determining the final navigation path point of the driving route as the current preview point in response to the preview arc length of the current navigation path point being smaller than the preview distance.

If the current navigation path point is D_jAnd the nearest navigation path point is D_iPre-aiming arc length l between_ijIf the current pre-aiming distance L is less than the current pre-aiming distance L, the current navigation path point is D_jIs the final navigation path point D_NThat is, the current preview point O is as shown in fig. 9C, and at this time, the current preview point O and the nearest navigation path point are D_iThe length of the pre-aiming arc between the two is less than the pre-aiming distance.

Further, after the current preview point is determined, step S230 is: tracking the current preview point using a pure tracking control algorithm to travel along the virtual track. The pure tracking control algorithm is a conventional tracking algorithm in the field and is not described in detail.

Further, the automatic tracking control method may further include the step of determining a current preview distance, which may be calculated based on the current driving speed of the vehicle and the curvature of the nearest navigation path point.

The curvature of each navigation path point may be a curvature of a corresponding navigation path point acquired in advance in the test driving process.

Preferably, the current preview distance corresponding to the nearest navigation path point can be determined by using the preview distance calculation formula (3).

Wherein l_dIs the current pre-aiming distance, v is the current running speed of the vehicle, ρ is the curvature of the nearest navigation path point, c₁、c₂And c₃Are each a constant.

While, for purposes of simplicity of explanation, the methodologies are shown and described as a series of acts, it is to be understood and appreciated that the methodologies are not limited by the order of acts, as some acts may, in accordance with one or more embodiments, occur in different orders and/or concurrently with other acts from that shown and described herein or not shown and described herein, as would be understood by one skilled in the art.

According to another aspect of the present invention, there is provided an automatic tracking control apparatus, as shown in fig. 10, including a memory 1010 and a processor 1020.

The memory 1010 is used to store computer programs.

The processor 1020 is connected to the memory 1010 for executing a computer program on the memory 1010, which when executed performs the steps of the automatic tracking control method described above.

According to yet another aspect of the present invention, there is also provided a computer storage medium having a computer program stored thereon, which when executed, performs the steps of the automatic tracking control method as described above.

According to yet another aspect of the present invention, there is also provided an automatic tracking control system.

In one embodiment, as shown in FIG. 11, the automatic tracking control system may include an inertial satellite integrated navigation unit 110, a body sensor 120, an automatic tracking control unit 130, and a steering execution unit 140.

The inertial satellite integrated navigation unit 110 may receive satellite signals in real time, collect real-time data of an internal gyroscope and an accelerometer, combine differential positioning reference information forwarded by a serial port-ethernet conversion gateway, perform fusion calculation, and output positioning and attitude information of the electric car, so as to provide current driving state data of the vehicle to the automatic tracking control unit 130.

The body sensor may collect auxiliary information such as the speed of the electric car and the angle of the steering shaft in real time, so as to assist the automatic tracking control unit 130 in determining the steering control strategy.

The automatic tracking control unit 130 is configured to execute the steps of the automatic tracking control method in any of the above embodiments to obtain a steering angle policy, and send the steering angle policy to the steering execution unit 140.

The steering execution unit 140 may drive an execution mechanism of the vehicle to steer according to a steering shaft angle command output by the automatic tracking control unit 130, so as to track the current preview point.

Further, in practical implementations, the automatic tracking control system may also include other modules that are necessary or unnecessary.

In one embodiment, as shown in fig. 12, the automatic tracking control system may include an inertial satellite integrated navigation unit, a vehicle body sensor, a serial-ethernet switching gateway, a vehicle integration gateway, an automatic tracking control unit, a steering execution unit, and a self-built differential base station. The inertial satellite integrated navigation unit, the vehicle body sensor, the serial port-Ethernet conversion gateway, the whole vehicle fusion gateway, the automatic tracking control unit and the steering execution unit are vehicle internal modules, and the self-built differential base station is public equipment arranged around a running route of a vehicle.

The inertial satellite integrated navigation unit can receive satellite signals in real time, collect real-time data of an internal gyroscope and an accelerometer, combine differential positioning reference information forwarded by a serial port-Ethernet conversion gateway, perform fusion calculation and output positioning and attitude information of the electric car. For providing the current driving status data of the vehicle to the automatic tracking control unit.

The body sensor can acquire signals such as the speed of the electric car and the angle of a steering shaft in real time and is used for assisting the steering control strategy determined by the automatic tracking control unit, and the body sensor sends the signals to the whole car fusion gateway.

The serial port-Ethernet conversion gateway can forward differential positioning reference information output by the whole vehicle fusion gateway to the inertial satellite integrated navigation unit, and forward positioning and attitude information of the electric vehicle output by the inertial satellite integrated navigation unit to the automatic tracking control unit.

The whole vehicle fusion gateway is a bridge among a vehicle body sensor, a serial port-Ethernet conversion gateway, an automatic tracking control unit and a self-built differential base station, and is used for forwarding differential positioning reference information output by the self-built differential base station to the serial port-Ethernet conversion gateway and forwarding electric vehicle speed, steering shaft angle and other signals output by the vehicle body sensor to the automatic tracking control unit.

The automatic tracking control unit is used for executing the steps of the automatic tracking control method to obtain a steering shaft angle instruction and sending the steering shaft angle instruction to the steering execution unit.

The steering execution unit can drive an execution mechanism of the vehicle to steer according to a steering shaft angle instruction output by the automatic tracking control unit.

The self-built differential base station is responsible for maintaining data of the base station, and transmits differential positioning reference information to the whole vehicle fusion gateway through the vehicle-ground wireless private network.

Those of skill in the art would understand that information, signals, and data may be represented using any of a variety of different technologies and techniques. For example, data, instructions, commands, information, signals, bits (bits), symbols, and chips that may be referenced throughout the above description may be represented by voltages, currents, electromagnetic waves, magnetic fields or particles, optical fields or particles, or any combination thereof.

Those of skill would further appreciate that the various illustrative logical blocks, modules, circuits, and algorithm steps described in connection with the embodiments disclosed herein may be implemented as electronic hardware, computer software, or combinations of both. To clearly illustrate this interchangeability of hardware and software, various illustrative components, blocks, modules, circuits, and steps have been described above generally in terms of their functionality. Whether such functionality is implemented as hardware or software depends upon the particular application and design constraints imposed on the overall system. Skilled artisans may implement the described functionality in varying ways for each particular application, but such implementation decisions should not be interpreted as causing a departure from the scope of the present invention.

The various illustrative logical modules, and circuits described in connection with the embodiments disclosed herein may be implemented or performed with a general purpose processor, a Digital Signal Processor (DSP), an Application Specific Integrated Circuit (ASIC), a Field Programmable Gate Array (FPGA) or other programmable logic device, discrete gate or transistor logic, discrete hardware components, or any combination thereof designed to perform the functions described herein. A general purpose processor may be a microprocessor, but in the alternative, the processor may be any conventional processor, controller, microcontroller, or state machine. A processor may also be implemented as a combination of computing devices, e.g., a combination of a DSP and a microprocessor, a plurality of microprocessors, one or more microprocessors in conjunction with a DSP core, or any other such configuration.

The steps of a method or algorithm described in connection with the embodiments disclosed herein may be embodied directly in hardware, in a software module executed by a processor, or in a combination of the two. A software module may reside in RAM memory, flash memory, ROM memory, EPROM memory, EEPROM memory, registers, hard disk, a removable disk, a CD-ROM, or any other form of storage medium known in the art. An exemplary storage medium is coupled to the processor such the processor can read information from, and write information to, the storage medium. In the alternative, the storage medium may be integral to the processor. The processor and the storage medium may reside in an ASIC. The ASIC may reside in a user terminal. In the alternative, the processor and the storage medium may reside as discrete components in a user terminal.

In one or more exemplary embodiments, the functions described may be implemented in hardware, software, firmware, or any combination thereof. If implemented in software as a computer program product, the functions may be stored on or transmitted over as one or more instructions or code on a computer-readable medium. Computer-readable media includes both computer storage media and communication media including any medium that facilitates transfer of a computer program from one place to another. A storage media may be any available media that can be accessed by a computer. By way of example, and not limitation, such computer-readable media can comprise RAM, ROM, EEPROM, CD-ROM or other optical disk storage, magnetic disk storage or other magnetic storage devices, or any other medium that can be used to carry or store desired program code in the form of instructions or data structures and that can be accessed by a computer. Any connection is properly termed a computer-readable medium. For example, if the software is transmitted from a web site, server, or other remote source using a coaxial cable, fiber optic cable, twisted pair, Digital Subscriber Line (DSL), or wireless technologies such as infrared, radio, and microwave, then the coaxial cable, fiber optic cable, twisted pair, DSL, or wireless technologies such as infrared, radio, and microwave are included in the definition of medium. Disk (disk) and disc (disc), as used herein, includes Compact Disc (CD), laser disc, optical disc, Digital Versatile Disc (DVD), floppy disk and blu-ray disc where disks (disks) usually reproduce data magnetically, while discs (discs) reproduce data optically with lasers. Combinations of the above should also be included within the scope of computer-readable media.

The previous description is provided to enable any person skilled in the art to practice the various aspects described herein. It is to be understood that the scope of the invention is to be defined by the appended claims and not by the specific constructions and components of the embodiments illustrated above. Those skilled in the art can make various changes and modifications to the embodiments within the spirit and scope of the present invention, and these changes and modifications also fall within the scope of the present invention.

Claims

1. An automatic tracking control method is suitable for a vehicle with an automatic tracking function, the vehicle automatically tracks a virtual track corresponding to a driving route of the vehicle to drive in a tracking state, and a plurality of navigation path points are arranged on the virtual track corresponding to the driving route of the vehicle, the automatic tracking control method comprises the following steps:

determining a nearest navigation waypoint passed by the vehicle based on a current driving state of the vehicle;

determining a current preview point of the vehicle based on the nearest navigation path point and a current preview distance; and

utilizing a pure tracking control algorithm to determine a steering control strategy for the vehicle to track the current home point.

2. The automatic tracking control method of claim 1, wherein the current driving state includes a current position and a current heading of the vehicle, each of the navigation waypoints on the driving route has a pre-collected preset heading, and the determining a nearest navigation waypoint that the vehicle has passed through based on the current driving state of the vehicle includes:

responding to the vehicle in a searching state, and determining the nearest navigation path point based on the distance between each navigation path point and the current position and the course included angle of the navigation path point; and

and responding to the fact that the vehicle is in a search-free state, and judging whether to update the next navigation path point to be the nearest navigation path point or not based on the vector relation from the current nearest navigation path point to the next navigation path point and the mass center of the vehicle, wherein the current nearest navigation path point is the nearest navigation path point determined in the last judging period.

3. The method as claimed in claim 2, wherein said determining the nearest navigation waypoint based on the distance from each navigation waypoint to the current location and the heading angle thereof comprises:

calculating the distance from each navigation path point to the current position one by one along the driving route and judging whether the navigation path point is a temporary nearest navigation path point in the currently calculated navigation path points; and

determining a most recently determined temporary most recent navigation waypoint as the most recent navigation waypoint until a final navigation waypoint to the travel route is calculated.

4. The automatic tracking control method as claimed in claim 3, wherein the distance calculation and determination process for each navigation path point comprises:

judging whether the distance from a current waypoint to the current position is smaller than the current nearest distance, wherein the current waypoint is a navigation waypoint of which the distance is being calculated, and the current nearest distance is the distance from a temporary nearest navigation waypoint determined before the current waypoint to the current position;

responding to the fact that the distance between the current waypoint and the current position is smaller than the current nearest distance, and judging whether a course included angle between the course of the current waypoint and the current course of the vehicle is smaller than 90 degrees or not;

in response to that the distance from the current waypoint to the current position is greater than or equal to the current nearest distance or the included angle between the course of the current waypoint and the current course of the vehicle is greater than or equal to 90 degrees, updating the next navigation waypoint of the current waypoint into the current waypoint in the next calculation and judgment process, and maintaining the temporary nearest navigation waypoint and the current nearest distance unchanged; and

and in response to the fact that the included angle between the current course of the current waypoint and the current course of the vehicle is smaller than 90 degrees, updating the current waypoint to be the temporary nearest navigation path point, updating the distance between the current waypoint and the current position to be the current nearest distance, and updating the next navigation path point of the current waypoint to be the current waypoint in the next calculation and judgment process.

5. The automatic tracking control method as claimed in claim 2, wherein the determining whether to update the next navigation waypoint to the closest navigation waypoint based on the vector relationship of the current closest navigation waypoint to the next navigation waypoint and the centroid of the vehicle comprises:

taking a vector formed by the current navigation path point pointing to the center of mass of the vehicle as a vehicle vector;

taking a vector formed by the current navigation path point pointing to the next navigation path point as a route vector;

calculating a judgment variable of the vehicle vector and the route vector

Wherein the coordinates of the center of mass of the vehicle is N (x)_n,y_n) The coordinate of the current nearest navigation path point is P (x)_k,y_k) The coordinate of the next navigation path point of the nearest navigation path point is Q (x)_k+1,y_k+1)；

Responding to the judgment variable value smaller than 1, and maintaining the current nearest navigation path point as the nearest navigation path point; and

and updating the next navigation path point to the nearest navigation path point in response to the judgment variable value being greater than or equal to 1.

6. The automatic tracking control method of claim 1, wherein said determining a current home point of the vehicle based on the closest navigation path point and a current home distance comprises:

starting from the nearest navigation path point, taking each navigation path point which is positioned on the driving route and behind the nearest navigation path point as a current navigation path point one by one, and judging whether the pre-aiming arc length from the current navigation path point to the nearest navigation path point is smaller than the pre-aiming distance and whether the current navigation path point is the final navigation path point of the driving route until the pre-aiming arc length from the current navigation path point to the nearest navigation path point is larger than or equal to the pre-aiming distance or the current navigation path point is the final navigation path point of the driving route;

in response to the fact that the pre-aiming arc length from the current navigation path point to the nearest navigation path point at the end is larger than or equal to the current pre-aiming distance, determining a point which is located between the current navigation path point and the previous navigation path point and has the pre-aiming arc length with the nearest navigation path point equal to the current pre-aiming distance by adopting a linear interpolation method, and determining the point as the current pre-aiming point; and

and in response to the aiming arc length of the current navigation path point at the end being smaller than the aiming distance, determining the final navigation path point of the driving route as the current aiming point.

7. The automatic tracking control method according to claim 6, wherein the navigation path points on the driving route are arranged at equal intervals, the distance between two adjacent navigation path points is a preset arc length, the navigation path points on the driving route are numbered in sequence, and the process of determining whether the pre-aiming arc length from the current navigation path point to the nearest navigation path point is smaller than the pre-aiming distance and whether the current navigation path point is the final navigation path point of the driving route each time comprises:

multiplying the difference between the number of the current navigation path point and the number of the nearest navigation path point by the preset arc length to obtain the pre-aiming arc length from the current navigation path point to the nearest navigation path point;

judging whether the pre-aiming arc length from the current navigation path point to the nearest navigation path point is smaller than the current pre-aiming distance and whether the number of the current navigation path point is smaller than the number of the final navigation path point of the driving route;

adding 1 to the number of the current navigation path point in response to that the pre-aiming arc length from the current navigation path point to the nearest navigation path point is less than the current pre-aiming distance and the number of the navigation path point is less than the number of the final navigation path point of the driving route;

and responding to the fact that the pre-aiming arc length from the current navigation path point to the nearest navigation path point is larger than or equal to the current pre-aiming distance or the number of the navigation path point is equal to the number of the final navigation path point of the driving route, and finishing the judgment.

8. The automatic tracking control method of claim 6, further comprising:

calculating the current pre-aiming distance based on the current driving speed of the vehicle and the curvature of the nearest navigation path point.

9. The automatic tracking control method of claim 8, wherein the calculating the current pre-aim distance based on the current travel speed of the vehicle and the curvature of the nearest navigation waypoint comprises:

using formula of calculation of pre-aiming distance

10. An automatic tracking control apparatus comprising a memory, a processor and a computer program stored on the memory, characterized in that the processor is adapted to carry out the steps of the automatic tracking control method according to any of claims 1-9 when executing the computer program stored on the memory.

11. A computer storage medium having a computer program stored thereon, wherein the computer program when executed implements the steps of the automatic tracking control method according to any of claims 1-9.

12. An automatic tracking control system, which is suitable for a vehicle with an automatic tracking function, wherein the vehicle automatically tracks a virtual orbit corresponding to a running route of the vehicle to run in a tracking state, a plurality of navigation path points are arranged on the virtual orbit corresponding to the running route of the vehicle, the automatic tracking control system comprises an inertial satellite combined navigation unit, a vehicle body sensor, an automatic tracking control unit and a steering execution unit, the automatic tracking control unit is respectively connected with the inertial satellite combined navigation unit, the vehicle body sensor and the steering execution unit, the inertial satellite combined navigation unit is used for determining the current running state of the vehicle, the vehicle body sensor is used for providing auxiliary state information to the automatic tracking control unit, and the automatic tracking control unit is used for implementing the steps of the automatic tracking control method according to any one of claims 1 to 9 so as to determine a steering control strategy of the vehicle And the steering execution unit is used for executing the steering control strategy.