CN114384902B - Automatic tracking control method and system thereof - Google Patents

Automatic tracking control method and system thereof Download PDF

Info

Publication number
CN114384902B
CN114384902B CN202011116729.7A CN202011116729A CN114384902B CN 114384902 B CN114384902 B CN 114384902B CN 202011116729 A CN202011116729 A CN 202011116729A CN 114384902 B CN114384902 B CN 114384902B
Authority
CN
China
Prior art keywords
navigation path
current
path point
nearest
point
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202011116729.7A
Other languages
Chinese (zh)
Other versions
CN114384902A (en
Inventor
张新锐
袁希文
张沙
黄瑞鹏
张乾
胡云卿
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
CRRC Zhuzhou Institute Co Ltd
Original Assignee
CRRC Zhuzhou Institute Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by CRRC Zhuzhou Institute Co Ltd filed Critical CRRC Zhuzhou Institute Co Ltd
Priority to CN202011116729.7A priority Critical patent/CN114384902B/en
Publication of CN114384902A publication Critical patent/CN114384902A/en
Application granted granted Critical
Publication of CN114384902B publication Critical patent/CN114384902B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory

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 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, and the automatic tracking control method comprises the following steps: determining a most recent navigation path point traversed by the vehicle based on a current travel state of the vehicle; determining a current pre-aiming point of the vehicle based on the nearest navigation path point and the current pre-aiming distance; and utilizing a pure tracking control algorithm to determine a steering control strategy for the vehicle to track the current pretightening point.

Description

Automatic tracking control method and system thereof
Technical Field
The invention relates to the field of vehicle steering control, in particular to an automatic tracking control method and an automatic tracking control system.
Background
Electric cars are common public transportation passenger vehicles, including rail cars, light rail cars, and trams. The existing tramcar, light tramcar and tramcar need special power system and track to cooperate to realize running, and infrastructure construction and vehicle acquisition cost are high. In order to solve the problem, the middle vehicle group proposes an electric car concept capable of tracking a virtual track on the ground, and the novel electric car cancels a steel rail and runs along the virtual track on the ground in a manner of rubber wheel bearing and steering.
The ground virtual track can be flexibly arranged, special infrastructure construction is not needed on the ground, and the novel trolley running virtual track is only needed to be generally drawn on the ground like a lane line and a zebra crossing. The novel trolley is not required to travel along the fixed track, so that the capital cost is greatly reduced, and the novel trolley has huge operation advantages relative to the trolley. Meanwhile, the running characteristics of the novel electric vehicle, namely the shared road right and the mixed traffic, enable the traffic system to have the advantage of flexible organization in the aspects of ground lane layout and the like.
The electric car developed by the car corporation in 2016 has realized an automatic tracking function based on vision, and the intelligent level of the novel electric car is further improved. The intelligent express system of 6 months in 2017 is released, the intelligent express system of 6 months in 2019 is opened by T1 line, the novel electric car is operated in three cities of Hunan Kangzhou, jiangxi Yongzhu, sichuan Yibin and the like successively, the requirements of customers on the tracking function are gradually improved, and the requirements on the safety and reliability of the tracking system are higher from initial tracking auxiliary to current full-line full-period tracking driving.
The automatic tracking function has become a vital function in the automatic driving or auxiliary driving of the novel electric car. In the field of vehicle autopilot profession, navigation-based lateral control theory has been studied intensively by the former, and examples of successful loading to achieve a control effect are also presented.
Among them, the pure tracking control algorithm has wide application in the path tracking control of unmanned vehicles. When the algorithm is used for tracking the straight line road section, the tracking speed is high, the precision is high, and the algorithm is matched with the running scene of the novel trolley. The pure tracking control algorithm requires that the desired track is known, and can be obtained by actually tracking the navigation path points on the preset virtual track to serve as the desired track, or can be obtained by utilizing the piecewise fitting curve of the navigation path points on the preset virtual track. To improve the stability of pure tracking control, the density of navigation path points on the virtual track may be increased or the fit curve may be ensured to be continuous everywhere, but the consumption of computing resources may be correspondingly increased.
The existing processors equipped on the novel vehicles have low processing capacity, and are difficult to be qualified for real-time calculation and real-time curve fitting of a huge number of path points in a short time. In order to solve the problem of lower 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 adapted to a vehicle having an automatic tracking function, the vehicle automatically tracking a virtual track corresponding to a travel route of the vehicle in a tracking state, the virtual track corresponding to the travel route of the vehicle being provided with a plurality of navigation path points, the automatic tracking control method comprising: determining a most recent navigation path point traversed by the vehicle based on a current travel state of the vehicle; determining a current pre-aiming point of the vehicle based on the nearest navigation path point and the current pre-aiming distance; and utilizing a pure tracking control algorithm to determine a steering control strategy for the vehicle to track the current pretightening point.
In an embodiment, the current driving state includes a current position and a current heading of the vehicle, each navigation path point on the driving route has a preset heading acquired in advance, and determining a nearest navigation path point traversed 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 from each navigation path point to the current position and the course included angle of the nearest navigation path point; and responding to the vehicle in a searching-free state, judging whether to update the next navigation path point to be the nearest navigation path point 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.
In an embodiment, the 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.
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 path point with the distance being calculated, and the current nearest distance is the distance from a temporary nearest navigation path point determined before the current waypoint to the current position; responding to the fact that the distance from the current road point to the current position is smaller than the current nearest distance, and judging whether the heading included angle between the heading of the current road point and the current heading of the vehicle is smaller than 90 degrees or not; in response to the distance from the current waypoint to the current position being 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 being greater than or equal to 90 degrees, updating the next navigation path point of the current waypoint to be the current waypoint in the next calculation and judgment process, and maintaining the temporary nearest navigation path point and the current nearest distance unchanged; and in response to the angle between the heading of the current waypoint and the current heading of the vehicle being smaller than 90 degrees, updating the current waypoint to be the temporary nearest navigation path point, updating the distance from the current waypoint to 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 an embodiment, 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: pointing at the centroid of the vehicle with the current navigation path pointIs used as a vehicle vector; taking a vector formed by pointing to the next navigation path point of the current navigation path point as a route vector; calculating the judgment variables of the vehicle vector and the route vectorWherein the coordinates of the centroid of the vehicle are N (x n ,y n ) The coordinates of the current nearest navigation path point are P (x k ,y k ) The coordinates of the next navigation path point of the nearest navigation path point are Q (x) k+1 ,y k+1 ) The method comprises the steps of carrying out a first treatment on the surface of the In response to the value of the decision variable being less than 1, maintaining the current nearest navigation waypoint as the nearest navigation waypoint; and updating the next navigation path point to the nearest navigation path point in response to the value of the judgment variable being greater than or equal to 1.
In an embodiment, the determining the current pre-aiming point of the vehicle based on the closest navigation path point and the current pre-aiming distance includes: starting from the nearest navigation path point, taking each navigation path point which is positioned on the driving route and is positioned behind the nearest navigation path point as a current navigation path point, judging whether the pretightening arc length from the current navigation path point to the nearest navigation path point is smaller than the pretightening distance and whether the current navigation path point is the final navigation path point of the driving route, and ending until the pretightening arc length from the current navigation path point to the nearest navigation path point is greater than or equal to the pretightening distance or the current navigation path point is the final navigation path point of the driving route; in response to the fact that the pretightening arc length from the current navigation path point to the nearest navigation path point at the end is greater than or equal to the current pretightening distance, determining a point which is positioned between the current navigation path point and the previous navigation path point and is equal to the current pretightening distance with the nearest navigation path point by adopting a linear interpolation method, and determining the point as the current pretightening point; and determining a final navigation path point of the travel route as the current pre-aiming point in response to the pre-aiming arc length of the current navigation path point at the end being less than the pre-aiming 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 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 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 preset 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; in response to the current navigation path point to the nearest navigation path point having a pre-aiming arc length less than the current pre-aiming distance and a number of the navigation path point less than a number of a final navigation path point of the travel route, adding 1 to the number of the current navigation path point; and responding to the preset arc length from the current navigation path point to the nearest navigation path point to be more than or equal to the current preset distance or the serial number of the navigation path point to be equal to the serial number of the final navigation path point of the driving route, and ending judgment.
In an embodiment, the automatic tracking control method further includes: and calculating the current pre-aiming distance based on the current running speed of the vehicle and the curvature of the nearest navigation path point.
In an embodiment, the calculating the current pretightening distance based on the current travel speed of the vehicle and the curvature of the nearest navigation path point includes: by using a pre-aiming distance calculation formulaDetermining a current pre-aiming distance corresponding to the nearest navigation path point, wherein l is as follows d For the current pre-aiming distance, v is the current running speed of the vehicle, ρ is the curvature of the nearest navigation path point, c 1 、c 2 And c 3 Respectively constant.
According to another aspect of the present invention, there is also provided an automatic tracking control apparatus including 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 as described in any one of the above when executing the computer program stored on the memory.
According to still another aspect of the present invention, there is also provided a computer storage medium having stored thereon a computer program which, when executed, implements the steps of the automatic tracking control method as set forth in any one of the above.
According to still another aspect of the present invention, there is provided an automatic tracking control system adapted to a vehicle having an automatic tracking function, the vehicle automatically tracking a virtual track corresponding to a travel route of the vehicle in a tracking state, a plurality of navigation path points being arranged on the virtual track corresponding to the travel route of the vehicle, the automatic tracking control system including 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 being respectively connected to the inertial satellite integrated navigation unit, the vehicle body sensor, and the steering execution unit, the inertial satellite integrated navigation unit being adapted to determine a current travel state of the vehicle, the vehicle body sensor being adapted to provide auxiliary state information to the automatic tracking control unit, the automatic tracking control unit being adapted to implement the steps of the automatic tracking control method as set forth in any one of the above to determine a steering control strategy of the vehicle, the steering execution unit being adapted to execute the steering control strategy.
Drawings
The above features and advantages of the present invention will be better understood after reading the detailed description of embodiments of the present disclosure in conjunction with the following drawings.
FIG. 1 is a schematic illustration of a travel route in a hypothetical scenario according to one aspect of the present invention;
FIG. 2 is a flow chart of an automatic tracking control method according to an embodiment of the invention;
FIG. 3 is a partial flow chart of an automatic tracking control method according to an embodiment of the invention;
FIG. 4 is a partial flow chart of an automatic tracking control method according to an embodiment of the invention;
FIGS. 5A-5C are schematic diagrams illustrating projected location relationships of vehicle vectors in different situations according to one aspect of the present invention;
FIG. 6 is a partial flow chart of an automatic tracking control method according to an embodiment of the invention;
FIG. 7 is a partial flow chart of an automatic tracking control method according to an embodiment of the invention;
FIG. 8 is a partial flow chart of an automatic tracking control method according to an embodiment of the invention;
9A-9C are schematic diagrams of current navigation routing point versus pre-sight point in various situations, depicted in accordance with an aspect of the present invention;
FIG. 10 is a block diagram of an automatic tracking control device according to another embodiment of the present invention;
FIG. 11 is a block diagram of an automatic tracking control system according to an embodiment of the present invention;
FIG. 12 is a block diagram of an automatic tracking control system according to an embodiment of the present invention.
Detailed Description
The following description is presented to enable one skilled in the art to make and use the invention and to incorporate it into the context of a particular application. Various modifications, as well as various uses in different applications will be readily apparent to persons 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 invention may be practiced without limitation 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 is directed to all documents and documents filed concurrently with this specification and open to public inspection with this specification, and the contents of all such documents 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 set of equivalent or similar features.
Note that where used, the designations left, right, front, back, top, bottom, forward, reverse, clockwise, and counterclockwise are used for convenience only and do not imply any particular orientation of securement. In fact, they are used to reflect the relative position and/or orientation between the various parts of the object. Furthermore, the terms "first," "second," and the like, 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 explicitly specified and limited otherwise, the terms "mounted," "connected," and "connected" are to be construed broadly, and may be either fixedly connected, detachably connected, or integrally connected, for example; 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.
Note that, where used, further, preferably, further and more preferably, the brief description of another embodiment is made on the basis of the foregoing embodiment, and further, preferably, further or more preferably, the combination of the contents of the rear band with the foregoing embodiment is made as a complete construction of another embodiment. A further embodiment is composed of several further, preferably, still further or preferably arrangements of the strips after the same embodiment, which may be combined arbitrarily.
The invention is described in detail below with reference to the drawings and the specific embodiments. It is noted that the aspects described below in connection with the drawings and the specific embodiments are merely exemplary and should not be construed as limiting the scope of the invention in any way.
According to one aspect of the present invention, an automatic tracking control method is provided, which is suitable for a vehicle having an automatic tracking function. The vehicle automatically tracks the virtual track on the road surface in the tracking state, and as shown in fig. 1, assuming that virtual tracks T1 and T2 are drawn for the tracked vehicle on the road, a plurality of navigation path points D may be further arranged on the virtual track T1 corresponding to the driving route of the vehicle. It will be appreciated that the virtual track T1 or T2 is a line or line segment (illustrated in fig. 1 as a simple line) having no direction, and that a segment of the virtual track may be shared by vehicles having different travel routes. The running route is a route with a direction formed by combining a plurality of virtual tracks, and the vehicle of the running route runs along the virtual track corresponding to the running route.
In one embodiment, as shown in FIG. 2, the automatic tracking control method 200 includes steps S210-S230.
Wherein, step S210 is: a most recent navigation path point traversed by the vehicle is determined based on a current travel state of the vehicle.
As shown in fig. 1, it is assumed that a virtual track corresponding to a driving route of a vehicle a is T1, and a current location of the vehicle a is a navigation route point D i And D i+1 The nearest navigation path point passed by the vehicle A is the navigation path point D i 。
The current running state is used for indicating the driving condition of the vehicle and at least comprises state data for indicating the position of the vehicle on a running route and state data of a driving direction.
In the prior art, a conventional method for determining the closest navigation route point to the vehicle on the driving route is to calculate the distances from all navigation route points on the driving route to the vehicle and determine the navigation route point with the smallest distance as the closest navigation route point every time it traverses. Although the conventional method is simple in principle, square and square opening are needed to be carried out for many times, the calculation process is complex, the calculation amount is large, and the time consumption is long. To solve the problems existing in the conventional method, the present invention classifies the vehicle into a search state and a no-search state, corresponding to the cases 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 existing conventional method is adopted to calculate the nearest navigation path point of the vehicle. That is, in response to the vehicle being in a search state, the nearest navigation waypoint may be determined based on the distance of each navigation waypoint to the current location of the vehicle and its heading angle.
It will be appreciated that the heading of the vehicle may indicate the direction of extension of the travel route, and the current location of the vehicle may then be used to determine the distance of each navigation path point from the vehicle.
Because the navigation path points are arranged along the driving route, the test vehicle can be adopted to carry out 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 in the test process.
It will be appreciated that since the vehicle needs to travel along the travel route, and the heading of the nearest navigation waypoint of the vehicle is almost the same as the extending direction of the travel route, the heading angle between the heading of the vehicle and the heading of the nearest navigation waypoint should not exceed 90 °, and if the heading angle between a certain navigation waypoint and the heading of the vehicle exceeds 90 °, it indicates that 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 by 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 running route of the vehicle and judging whether each navigation path point is the temporary nearest navigation path point in the currently calculated navigation path points.
Step S320 is: and until a final navigation path point of the driving route is calculated, determining a temporary nearest navigation path point in the currently calculated navigation path points as the nearest navigation path point.
Assume that a traveling route of a vehicle includes a navigation route point D 1 ～D N The navigation path points D 1 ～D N Are arranged in sequence according to the number along the driving route. Then first D is 1 Determining a temporary nearest navigation path point; and then judge D 1 And D 2 A temporary nearest navigation path point in (a); and then judge D 1 ～D 3 The temporary nearest navigation path point in (C) is judged to be D 1 ～D 4 And so on until D is determined 1 ～D N The last determined temporary nearest navigation path point is the actual nearest navigation path point of the current position of the vehicle.
Specifically, at any navigation path point D i (1 < i < N) for example, the distance calculation and judgment process of each navigation path point is described. Navigation Path Point D i Previously determined D 1 ～D i-1 The temporary nearest navigation path point in (a) is D T (1.ltoreq.T.ltoreq.i-1), the current closest distance being the temporary closest navigation path point D T Distance L to the current position of the vehicle T . Then judge D 1 ～D i The procedure of the temporary closest navigation path point in (a) may include steps S311 to S314 as shown in fig. 4.
Wherein, step S311 is: and judging whether the distance from the current waypoint to the current position is smaller than the current nearest distance, wherein the current waypoint is a navigation path point with the distance being calculated, and the current nearest distance is the distance from the temporary nearest navigation path point determined before the current waypoint to the current position.
Then at navigation path point D i In the distance calculation and judgment process of (a), step S311 is: judging navigation path point D i Whether the distance to the current position of the vehicle is less than the current closest distance L T 。
If the navigation route point D i The distance to the current position of the vehicle is less than the current closest distance L T Step S312 is executed if the navigation path point D i The distance to the current position of the vehicle is greater than or equal to the current closest distance L T Step S313 is performed.
Step S312 is: and in response to the distance from the current road point to the current position being smaller than the current nearest distance, judging whether the heading included angle between the heading of the current road point and the current heading of the vehicle is smaller than 90 degrees.
The current course of the vehicle is the course 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 acquired by an inertial satellite combined navigation unit configured on the test vehicle at the current waypoint position in the pre-acquisition test driving process of the navigation waypoint of the test vehicle. The course included angle between the course of the current road point and the current course of the vehicle is the included angle of the two courses under the same coordinate system.
It will be appreciated that a heading angle of the current waypoint of less than 90 ° from the current heading of the vehicle indicates that the vehicle is still traveling along its travel route, is not off the travel route and is not in the opposite direction of travel from the travel route.
At navigation path point D i In the distance calculation and judgment process of (a), step S312 is: judging navigation path point D i Whether the angle between the heading of the vehicle at its current position is less than 90 deg.. If the navigation route point D i The step S314 is performed, in which the angle between the heading of the vehicle and the heading of the vehicle at its current position is smaller than 90 °; if the navigation route point D i The step S313 is performed such that the angle between the heading of the vehicle and the heading of the vehicle at its current position is greater than or equal to 90 °.
Step S313 is: and in response to the fact that the distance from the current road point to the current position is greater than or equal to the current nearest distance or the included angle between the heading of the current road point and the current heading of the vehicle is greater than or equal to 90 degrees, updating the next navigation path point of the current road point to be the current road point in the next calculation and judgment process, and maintaining the temporary nearest navigation path point and the current nearest distance unchanged.
I.e. at navigation path point D i When the navigation path point D is in the distance calculation and judgment process of (1) i The distance to the current position of the vehicle is greater than or equal to the current nearest distance L T Or navigation waypoint D i When the included angle between the heading of the vehicle and the heading of the vehicle at the current position is more than or equal to 90 DEG, D is calculated i+1 Updating the current road point calculated and judged next time and maintaining the navigation path point D T For temporarily nearest navigation waypoint and navigation waypoint D T Distance L to the current position of the vehicle T The current nearest distance is unchanged.
Step S314 is: and in response to the fact that the course included angle between the course of the current road point and the course of the current course of the vehicle is smaller than 90 degrees, updating the current road point to be a temporary nearest navigation path point, updating the distance from the current road point to the current position of the vehicle to be the current nearest distance, and updating the next navigation path point of the current road point to be the current road point in the next calculation and judgment process.
I.e. at navigation path point D i When the navigation path point D is in the distance calculation and judgment process of (1) i The distance to the current position of the vehicle is less than the current closest distance L T And navigate waypoint D i When the included angle between the heading of the vehicle at the current position is smaller than 90 DEG, D is calculated i Updated to D 1 ～D i Temporary nearest navigation Path Point in (D) i Distance L to the current position of the vehicle i Updated to D 1 ～D i And will D i+1 Updating the current road point to the next calculated and judged current road point.
In particular, in the calculation and judgment process of the first navigation route point of the traveling route, since only one navigation route point exists, the first navigation route point is directly updated to be the temporary nearest navigation route point, the distance from the first navigation route point to the current position of the vehicle is updated to be the current nearest distance calculated and judged next, and the second navigation route point is updated to be the current route point calculated and judged next.
In particular, in the calculation and judgment process of 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 smaller than the current nearest distance and the angle between the heading of the last navigation path point and the heading of the current heading of the vehicle is smaller than 90 degrees, the last navigation path point is updated to be the nearest navigation path point of the vehicle to finish, otherwise, the temporary nearest navigation path point obtained in the previous calculation and judgment process, namely the nearest determined temporary nearest navigation path point is determined to be the nearest navigation path point of the vehicle to finish.
Correspondingly, when the current nearest navigation waypoint of the vehicle is known, the vehicle is in a search-free state, and whether the next navigation waypoint needs to be updated to the nearest navigation waypoint can be judged based on the vector from the known current nearest navigation waypoint to the next navigation waypoint and the vector from the current nearest navigation waypoint to the mass center of the vehicle. That is, it is determined whether or not the latest navigation route point needs to be updated based on the determined latest navigation route point.
As shown in fig. 5A to 5C, the centroid coordinates of the vehicle are assumed to be N (x n ,y n ) Point, P (x) k ,y k ) And Q (x) k+1 ,y k+1 ) The navigation route points are respectively the kth navigation route point and k+1 navigation route points on the driving route, the S point is the projection point of the N point on the straight line where the PQ is located, and the line segment formed by connecting the N point and the PQ has three position relations.
(Vector)Is->The projection on the line segment PQ has directivity. Define a rule for judging->And line segment PQVariable R of relation n Variable R n The calculation formula of (2) is as follows:
as can be seen from FIG. 5, when R n When less than 0, the method belongs to the (1) case; when 0 < R n When less than 1, the method belongs to the (2) case; when R is n When the number is more than 1, the case of the type (3); when R is n When=0, the S point coincides with the P point, belonging to the (2) th case; when R is n When=1, the S point coincides with the Q point, and the case belongs to the (2) th case.
The P point is assumed to be a known nearest navigation path point of the vehicle, and may specifically be a nearest navigation path point determined in the above searching state or a nearest navigation path point determined in the last vector determination process. Then when R n And when the navigation route point Q is not less than 1, the train passes through the navigation route point P, and the next navigation route point Q of the P point can be updated to be the nearest navigation route point.
Assume that a traveling route of a vehicle includes a navigation route point D 1 ～D N The navigation path points D 1 ～D N Arranged in serial number along the driving route, then using any nearest navigation route point D i (1 < i < N) for example, the update process of the nearest navigation route point in the search-free state is described.
As shown in fig. 6, the updating process of any nearest navigation path point in the search-free state may include steps S610 to S650.
Wherein S610 is: the vector formed by the current navigation path point pointing to the mass center of the vehicle is taken as a vehicle vector.
Taking any one of fig. 5A to 5C as an example, assume the nearest navigation path point D i Is P (x) k ,y k ) The coordinates of the centroid of the vehicle are N (x n ,y n ) The car vector is the directed line segment PN.
S620 is: and taking a vector formed by pointing the current navigation path point to the next navigation path point as a route vector.
Taking any one of figures 5A-5C as an example,assume the nearest navigation path point D i Is P (x) k ,y k ) Nearest navigation path point D i Is Q (x k+1 ,y k+1 ) The route vector is a directed segment PQ.
S630 is: and calculating judgment variables of the vehicle vector and the route vector. Coordinates N (x of centroid of vehicle n ,y n ) Coordinates P (x of the current nearest navigation path point k ,y k ) And the coordinates Q (x k+1 ,y k+1 ) Substituted into (1) to determine the variable R n Is a value of (2).
Taking any one of fig. 5A to 5C as an example, assuming that a projection point of the centroid of the vehicle onto a line where the line segment PQ is located is S, the projection vector is a directional line segment PS, it can be understood that when the direction of the directional line segment PS and the direction of the route vector are the same, (x) k+1 -x k )·(x n -x k )+(y k+1 -y k )·(y n -y k ) Is positive, R n > 0, R is the length of segment PS greater than or equal to the modulus of the route vector n 1 or more, can correspond to the case shown in FIG. 5C, if the length of segment PS is smaller than the modulus of the route vector, R n < 1, which may correspond to the situation shown in FIG. 5B; if the direction of the directional segment PS is opposite to the direction of the route vector, (x) k+1 -x k )·(x n -x k )+(y k+1 -y k )·(y n -y k ) Negative, R n < 0, which may correspond to the situation shown in FIG. 5A.
Step S640 is: and in response to the value of the judgment variable being less than 1, maintaining the current nearest navigation path point as the nearest navigation path point.
To determine whether to update the current nearest navigation path point D i For example, if the value of the judgment variable is less than 1, the current nearest navigation path point D is maintained i The navigation path point is unchanged for the nearest navigation.
Step S650 is: and in response to the value of the judgment variable being greater than or equal to 1, updating the next navigation path point to the nearest navigation path point.
To determine whether to update the current latest navigation pathPoint D i For example, if the value of the judgment variable is greater than or equal to 1, the current nearest navigation path point D i Is the next navigation waypoint D of (1) i+1 Updated to the nearest navigation path point and the navigation path point D is used in the next updating judgment process i+1 Is the current nearest navigation path point.
By combining the searching state and the searching-free state of the vehicle, after the latest navigation path point is determined through the determining step of the latest navigation path point in the searching state for the first time, the method can judge whether the vehicle passes over the current latest navigation path point by adopting a vector projection method in the searching-free state, and if so, the latest navigation path point is updated. The combination is free from square and square for many times, the calculated amount is small, the program realization efficiency is high, the processing operation resource consumption is remarkably reduced, more navigation path point calculation can be supported under the condition of limited processing operation resource consumption, and the control effect of the pure tracking control algorithm is better when the combination corresponds to the same section of driving route and can support higher navigation path point density.
Further, after determining the nearest navigation path point of the vehicle, step S220 is: and determining the current pre-aiming point of the vehicle based on the nearest navigation path point and the current pre-aiming distance.
The pretightening point is a tracking target of a pure tracking algorithm, and the pure tracking algorithm tracks the corresponding pretightening point at different positions to realize the determination of the steering control strategy. The current pretightening point refers to a nearest navigation path point corresponding to the current position of the vehicle and a pretightening point associated with the current pretightening distance.
The pretightening distance is mainly used for determining pretightening points according to curvature change of a running route where the vehicle is and all changes of the current running speed, and generally, a point which is on the running route and has a pretightening distance with the nearest navigation path point is the pretightening point by taking the nearest navigation path point as a starting point.
Since the navigation route points are pre-arranged navigation aid marks on the travel route, the distance between two adjacent navigation route points on the travel route is pre-designed, i.e. known, or the distance between any two adjacent navigation route points is equal. Thus, the pre-aiming arc length between the nearest navigation path point and any one of the following navigation path points can be determined based on the preset arc lengths 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 approximately determined based on the magnitude relation between the pre-aiming arc length and the pre-aiming distance.
The arc length refers to the length of an arc formed by a road segment between any two navigation path points when the driving route passes through the two navigation path points. The preset arc length is a known arc length between two adjacent navigation path points, and may be a fixed arc length when the navigation path points on the travel route are equidistantly arranged. When judging whether the pre-aiming point is between the nearest navigation path point and the other navigation path point, the pre-aiming arc length refers to the length of an arc formed by a road section of the running route between the nearest navigation path point and the other navigation path point.
Specifically, as shown in fig. 7, step S220 may include steps S221 to S223.
Step S221 is: and taking each navigation path point which is positioned on the driving route and is positioned behind the nearest navigation path point one by one from the nearest navigation path point as a current 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 pre-aiming distance and whether the current navigation path point is the final navigation path point of the driving route or not until the pre-aiming arc length from the current navigation path point to the nearest navigation path point is greater than or equal to the pre-aiming distance or the current navigation path point is the final navigation path point of the driving route.
Assume that a traveling route of a vehicle includes a navigation route point D 1 ～D N The navigation path points D 1 ～D N The serial numbers of the navigation path points D are sequentially 1-N 1 ～D N The navigation route points D are arranged in sequence according to numbers along the driving route i (1. Ltoreq.i. Ltoreq.N) as the nearest navigation path point, step S221 first determines D i+1 From the nearest navigation path point D i Whether the pretarget arc length of (2) is smaller than the pretarget distance and D i+1 Whether or not to be the final of the driving routeNavigation route point, if D i+1 From the nearest navigation path point D i Is greater than or equal to the pretightening arc length or D i+1 If the navigation path point is the final navigation path point of the driving route, ending the judgment, and if D i+1 From the nearest navigation path point D i Is smaller than the pretarge distance and D i+1 If the navigation path point is not the final navigation path point of the driving route, continuing to judge D i+2 From the nearest navigation path point D i Whether the pretarget arc length of (2) is smaller than the pretarget distance and D i+2 Whether the navigation route is the final navigation route point of the driving route; and so on until the end condition is met.
The process of determining whether the pre-aiming arc length from any navigation route point located after the nearest navigation route point to the nearest navigation route point is smaller than the pre-aiming distance and whether the navigation route point is the final navigation route point of the driving route may be as shown in fig. 8, and includes 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 the route point D i As the nearest navigation path point D i To be located at the nearest navigation path point D i Any navigation path point D thereafter j For example, the determining process of (i.ltoreq.j.ltoreq.N), the step S810 may specifically correspond to: the nearest navigation path point D i And navigation waypoint D j The number of (2) is substituted into the pre-aiming arc length calculation formula to calculate the navigation path point D j And the nearest navigation path point D i The pre-aiming arc length between the two.
l ij ＝(j-i)×l 0 (2)
Wherein l ij For navigating the waypoint D j And the nearest navigation path point D i The pretarge arc length between the two points, i is the nearest navigation path point D i J is the navigation path point D j Number of (l) 0 Is a preset arc length.
In order to solve the current pre-aiming 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 larger than or equal to the pre-aiming distance, the traversed current navigation path point is used as the final current navigation path point. According to the invention, by arranging the equally-spaced road points, the pre-aiming arc length is calculated by using the formula (2). The calculation amount is small, the code realization efficiency is high, the processing operation resource consumption is remarkably reduced, more navigation path point calculation can be supported under the condition of limited processing operation resource consumption, and the control effect of the pure tracking control algorithm is better when the calculation corresponds to the same section of driving route and can support higher navigation path point density.
Step S820 is: and 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.
To navigate the route point D i As the nearest navigation path point D i To be located at the nearest navigation path point D i Any navigation path point D thereafter j For example, assuming that the current pretightening distance is L, step S820 may specifically correspond to: judging the pre-aiming arc length l ij Whether is smaller than the current pretightening distance L and j is smaller than N, if so, the pretightening arc length L ij If the preset arc length L is smaller than the current preset distance L and j is smaller than N, step S830 is performed ij If the preset distance L or j is greater than or equal to the current preset distance N, step S840 is performed.
Step S830 is: and in response to the preset arc length from the current navigation path point to the nearest navigation path point being less than the current preset distance and the number of the navigation path point being 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 the route point D i As the nearest navigation path point D i To be located at the nearest navigation path point D i Any navigation path point D thereafter j For example, the judgment process of (i.ltoreq.j.ltoreq.N), step S830 may haveThe body corresponds to: will be determined as D j+1 Determining as the current navigation path point for the next round D j+1 Is determined by the judgment process of (2).
Step S840 is: and responding to the preset arc length from the current navigation path point to the nearest navigation path point to be more than or equal to the current preset distance or the serial number of the navigation path point to be equal to the serial number of the final navigation path point of the driving route, and ending judgment.
To navigate the route point D i As the nearest navigation path point D i To be located at the nearest navigation path point D i Any navigation path point D thereafter j For example, the determining process of (i.ltoreq.j.ltoreq.N), step S840 may specifically correspond to: maintaining the current navigation waypoint still at D j 。
It can be understood that the latest current navigation path point determined in step S221 has two cases, one is that the pre-aiming arc length between the current navigation path point and the latest navigation path point is equal to or greater than the current pre-aiming 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 satisfying both of the foregoing cases, for which the current pre-aiming point should be located between the final navigation path point and its previous navigation path point, and thus it is necessary to determine the current pre-aiming point based on the magnitude relation of the pre-aiming arc length and the pre-aiming distance between the current navigation path point and the nearest navigation path point.
Accordingly, correspondingly, step S222 is: and in response to the fact that the pretightening arc length from the current navigation path point to the nearest navigation path point is greater than or equal to the current pretightening distance, determining a point which is positioned between the current navigation path point and the previous navigation path point and is equal to the current pretightening distance with the nearest navigation path point by adopting a linear interpolation method, and determining the point as the current pretightening point.
That is, suppose the nearest navigation route point is D i The current navigation path point at the end of step S221 is D j (i < j is less than or equal to N), if the current navigation path point is D j D is the point of the navigation path closest to i Pre-aiming arc length between ij Greater thanOr equal to the current pretightening distance L, then the current pretightening point O should be located at the same distance D as shown in FIG. 9A or 9B j-1 And D j On the driving route between them. Here, fig. 9A corresponds to a case where j < N, and fig. 9B corresponds to a case where j=n.
Step S223 is: and determining a final navigation path point of the driving route as the current pre-aiming point in response to the pre-aiming arc length of the current navigation path point being smaller than the pre-aiming distance.
If the current navigation path point is D j D is the point of the navigation path closest to i Pre-aiming arc length between ij If the current navigation path point is smaller than the current pre-aiming distance L, the current navigation path point is D j I.e. the final navigation path point D N Namely the current pretightening point O, as shown in FIG. 9C, the current pretightening point O and the nearest navigation path point are D i The pretightening arc length between the two is smaller than the pretightening distance.
Further, after determining the current pre-aiming point, step S230 is: tracking the current pre-aiming point by using a pure tracking control algorithm to drive along the virtual track. The pure tracking control algorithm is a conventional tracking algorithm in the field, and will not be described in detail.
Still further, the automatic tracking control method may further include a step of determining a current pretightening distance, which may be calculated based on a current traveling speed of the vehicle and a curvature of a 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 pretightening distance corresponding to the nearest navigation path point can be determined by using a pretightening distance calculation formula (3).
Wherein l d For the current pre-aiming distance, v is the current running speed of the vehicle, ρ is the curvature of the nearest navigation path point, c 1 、c 2 And c 3 Respectively 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 and appreciated by those skilled in the art.
According to another aspect of the present invention, there is also provided an automatic tracking control apparatus, as shown in fig. 10, including a memory 1010 and a processor 1020.
The memory 1010 is used for storing a computer program.
The processor 1020 is coupled to the memory 1010 for executing a computer program on the memory 1010 that, when executed, performs the steps of the automatic tracking control method described above.
According to still another aspect of the present invention, there is also provided a computer storage medium having stored thereon a computer program which, when executed, implements 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 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, and combine and calculate and output positioning and attitude information of an electric car for providing current running state data of the vehicle to the automatic tracking control unit 130.
The vehicle body sensor may collect auxiliary information such as a trolley speed and a steering shaft angle in real time for assisting the automatic tracking control unit 130 in determining a steering control strategy.
The automatic tracking control unit 130 is configured to perform the steps of the automatic tracking control method in any of the above embodiments to obtain the steering angle strategy, and send the steering angle strategy to the steering execution unit 140.
The steering execution unit 140 may drive an execution mechanism of the vehicle to steer according to the steering shaft steering angle command output by the automatic tracking control unit 130, thereby realizing tracking of the current pre-aiming point.
Further, in actual implementation, the automatic tracking control system may further include other necessary or unnecessary modules.
In an embodiment, as shown in fig. 12, the automatic tracking control system may include modules such as an inertial satellite integrated navigation unit, a vehicle body sensor, a serial port-ethernet conversion gateway, a vehicle integration gateway, an automatic tracking control unit, a steering execution unit, and a self-built differential base station. The system comprises an inertial satellite integrated navigation unit, a vehicle body sensor, a serial port-Ethernet conversion gateway, a whole vehicle fusion gateway, an automatic tracking control unit and a steering execution unit, wherein 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 the serial port-Ethernet conversion gateway, and fusion, calculate and output positioning and attitude information of the trolley bus. For providing the auto-tracking control unit with current driving state data of the vehicle.
The vehicle body sensor can acquire signals such as the speed of the trolley and the rotation angle of the steering shaft in real time and is used for assisting the steering control strategy determined by the automatic tracking control unit, and the vehicle body sensor is sent to the whole vehicle fusion gateway.
The serial port-Ethernet conversion gateway can forward the differential positioning reference information output by the whole vehicle fusion gateway to the inertial satellite integrated navigation unit, and forward the positioning and attitude information of the electric car 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, forwards differential positioning reference information output by the self-built differential base station to the serial port-Ethernet conversion gateway, and forwards signals such as the speed of a trolley, the rotation angle of a steering shaft and the like 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 the steering shaft steering angle instruction output by the automatic tracking control unit.
The self-built differential base station is responsible for maintaining the data of the reference 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, 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 (disk) as used herein include Compact Disc (CD), laser disc, optical disc, digital Versatile Disc (DVD), floppy disk and blu-ray disc where disks (disk) usually reproduce data magnetically, while discs (disk) 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 controlled by the appended claims and not limited to the specific constructions and components of the above-described embodiments. Various changes and modifications to the embodiments may be made by those skilled in the art within the spirit and scope of the invention, and such changes and modifications are intended to be included within the scope of the invention.

Claims (11)

1. An automatic tracking control method is applicable to a vehicle with an automatic tracking function, the vehicle automatically tracks virtual tracks corresponding to a driving route of the vehicle in a tracking state, a plurality of navigation path points are arranged on the virtual tracks corresponding to the driving route of the vehicle, and the automatic tracking control method comprises the following steps:
judging whether to update the next navigation path point to be the nearest navigation path point 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 in response to the vehicle being in a search-free state, wherein the current nearest navigation path point is the nearest navigation path point determined in the previous judging period;
Vector formed by pointing the current navigation path point to the mass center of the vehicleThe vector formed by pointing the current navigation path point to the next navigation path point is used as a route vector, and the judgment variables of the vehicle vector and the route vector are calculatedWherein the coordinates of the centroid of the vehicle are N (x n ,y n ) The coordinates of the current nearest navigation path point are P (x k ,y k ) The coordinates of the next navigation path point of the nearest navigation path point are Q (x) k+1 ,y k+1 )；
In response to the value of the decision variable being less than 1, maintaining the current nearest navigation waypoint as the nearest navigation waypoint;
updating the next navigation path point to the nearest navigation path point in response to the value of the judgment variable being greater than or equal to 1;
determining a current pre-aiming point of the vehicle based on the nearest navigation path point and the current pre-aiming distance; and
a pure tracking control algorithm is utilized to determine a steering control strategy for the vehicle to track the current pretightening point.
2. The automatic tracking control method according to claim 1, wherein the current running state includes a current position and a current heading of the vehicle, each navigation path point on the running route has a preset heading acquired in advance, the automatic tracking control method further comprising:
And responding to the vehicle in a searching state, and determining the nearest navigation path point based on the distance from each navigation path point to the current position and the heading included angle of the nearest navigation path point.
3. The automatic tracking control method according to claim 2, wherein the determining the nearest navigation path point based on the distance of each navigation path point to the current position and its heading angle 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
and until a final navigation path point to the driving route is calculated, determining the latest determined temporary latest navigation path point as the latest navigation path point.
4. The automatic tracking control method according to claim 3, wherein the distance calculating and judging process of 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 path point with the distance being calculated, and the current nearest distance is the distance from a temporary nearest navigation path point determined before the current waypoint to the current position;
Responding to the fact that the distance from the current road point to the current position is smaller than the current nearest distance, and judging whether the heading included angle between the heading of the current road point and the current heading of the vehicle is smaller than 90 degrees or not;
in response to the distance from the current waypoint to the current position being 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 being greater than or equal to 90 degrees, updating the next navigation path point of the current waypoint to be the current waypoint in the next calculation and judgment process, and maintaining the temporary nearest navigation path point and the current nearest distance unchanged; and
and in response to the angle between the heading of the current road point and the current heading of the vehicle being smaller than 90 degrees, updating the current road point to be the temporary nearest navigation path point, updating the distance from the current road point to the current position to be the current nearest distance, and updating the next navigation path point of the current road point to be the current road point in the next calculation and judgment process.
5. The automatic tracking control method according to claim 1, wherein the determining the current pre-aiming point of the vehicle based on the nearest navigation path point and the current pre-aiming distance includes:
Starting from the nearest navigation path point, taking each navigation path point which is positioned on the driving route and is positioned behind the nearest navigation path point as a current navigation path point, judging whether the pretightening arc length from the current navigation path point to the nearest navigation path point is smaller than the pretightening distance and whether the current navigation path point is the final navigation path point of the driving route, and ending until the pretightening arc length from the current navigation path point to the nearest navigation path point is greater than or equal to the pretightening distance or the current navigation path point is the final navigation path point of the driving route;
in response to the fact that the pretightening arc length from the current navigation path point to the nearest navigation path point at the end is greater than or equal to the current pretightening distance, determining a point which is positioned between the current navigation path point and the previous navigation path point and is equal to the current pretightening distance with the nearest navigation path point by adopting a linear interpolation method, and determining the point as the current pretightening point; and
and determining a final navigation path point of the driving route as the current pre-aiming point in response to the pre-aiming arc length of the current navigation path point at the end being smaller than the pre-aiming distance.
6. The automatic tracking control method according to claim 5, wherein the navigation path points on the travel 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 travel route are numbered sequentially, and the process of judging each time whether the preset arc length from the current navigation path point to the nearest navigation path point is smaller than the preset distance and whether the current navigation path point is the final navigation path point of the travel route 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 preset 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;
in response to the current navigation path point to the nearest navigation path point having a pre-aiming arc length less than the current pre-aiming distance and a number of the navigation path point less than a number of a final navigation path point of the travel route, adding 1 to the number of the current navigation path point;
And responding to the preset arc length from the current navigation path point to the nearest navigation path point to be more than or equal to the current preset distance or the serial number of the navigation path point to be equal to the serial number of the final navigation path point of the driving route, and ending judgment.
7. The automatic tracking control method according to claim 5, characterized by further comprising:
and calculating the current pre-aiming distance based on the current running speed of the vehicle and the curvature of the nearest navigation path point.
8. The automatic tracking control method according to claim 7, characterized in that the calculating the current pre-aiming distance based on the current running speed of the vehicle and the curvature of the nearest navigation path point includes:
by using a pre-aiming distance calculation formulaDetermining a current pre-aiming distance corresponding to the nearest navigation path point, wherein l is as follows d For the current pre-aiming distance, v is the current running speed of the vehicle, ρ is the curvature of the nearest navigation path point, c 1 、c 2 And c 3 Respectively constant.
9. An automatic tracking control device comprising a memory, a processor and a computer program stored on the memory, characterized in that the processor is adapted to implement the steps of the automatic tracking control method according to any one of claims 1-8 when executing the computer program stored on the memory.
10. 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 one of claims 1 to 8.
11. An automatic tracking control system 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 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 comprises 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 with 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 the current driving state of the vehicle, the vehicle body sensor is used for providing auxiliary state information for the automatic tracking control unit, the automatic tracking control unit is used for realizing the steps of the automatic tracking control method according to any one of claims 1-8 so as to determine the steering control strategy of the vehicle, and the steering execution unit is used for executing the steering control strategy.
CN202011116729.7A 2020-10-19 2020-10-19 Automatic tracking control method and system thereof Active CN114384902B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011116729.7A CN114384902B (en) 2020-10-19 2020-10-19 Automatic tracking control method and system thereof

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011116729.7A CN114384902B (en) 2020-10-19 2020-10-19 Automatic tracking control method and system thereof

Publications (2)

Publication Number Publication Date
CN114384902A CN114384902A (en) 2022-04-22
CN114384902B true CN114384902B (en) 2024-04-12

Family

ID=81193354

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011116729.7A Active CN114384902B (en) 2020-10-19 2020-10-19 Automatic tracking control method and system thereof

Country Status (1)

Country Link
CN (1) CN114384902B (en)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116736864A (en) * 2023-07-12 2023-09-12 北鲲睿航科技(上海)有限公司 Autonomous navigation control method and system

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109085823A (en) * 2018-07-05 2018-12-25 浙江大学 The inexpensive automatic tracking running method of view-based access control model under a kind of garden scene
CN109520498A (en) * 2017-09-18 2019-03-26 中车株洲电力机车研究所有限公司 A kind of virtual railway points system and method for virtual rail vehicle
CN110308717A (en) * 2018-03-27 2019-10-08 广州汽车集团股份有限公司 Control the mobile machine of autonomous type mobile method, apparatus, machine and storage medium
CN110398966A (en) * 2019-07-22 2019-11-01 广州小鹏汽车科技有限公司 A kind of path tracking control method and path following system
CN110667563A (en) * 2019-09-20 2020-01-10 北京汽车集团有限公司 Transverse control method and device for automatic driving vehicle and vehicle
CN110989625A (en) * 2019-12-25 2020-04-10 湖南大学 Vehicle path tracking control method

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
GB2492602A (en) * 2010-12-06 2013-01-09 Agco Corp Automatic determination of tire height for improving vehicle guidance performance

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109520498A (en) * 2017-09-18 2019-03-26 中车株洲电力机车研究所有限公司 A kind of virtual railway points system and method for virtual rail vehicle
CN110308717A (en) * 2018-03-27 2019-10-08 广州汽车集团股份有限公司 Control the mobile machine of autonomous type mobile method, apparatus, machine and storage medium
CN109085823A (en) * 2018-07-05 2018-12-25 浙江大学 The inexpensive automatic tracking running method of view-based access control model under a kind of garden scene
CN110398966A (en) * 2019-07-22 2019-11-01 广州小鹏汽车科技有限公司 A kind of path tracking control method and path following system
CN110667563A (en) * 2019-09-20 2020-01-10 北京汽车集团有限公司 Transverse control method and device for automatic driving vehicle and vehicle
CN110989625A (en) * 2019-12-25 2020-04-10 湖南大学 Vehicle path tracking control method

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
田间道路无人驾驶搬运车自动循迹行驶控制策略;孙红伟;《江苏农业科学》;第46卷(第7期);第215-219页 *

Also Published As

Publication number Publication date
CN114384902A (en) 2022-04-22

Similar Documents

Publication Publication Date Title
WO2021136130A1 (en) Trajectory planning method and apparatus
CN110160552B (en) Navigation information determination method, device, equipment and storage medium
WO2019052567A1 (en) Virtual turnout system and method for virtual rail vehicle
US10012995B2 (en) Autonomous vehicle routing and navigation using passenger docking locations
US9519290B2 (en) Associating passenger docking locations with destinations
US20200005645A1 (en) Orientation-Adjust Actions for Autonomous Vehicle Operational Management
CN101780774B (en) Route guidance device, route guidance method
BR112019016268B1 (en) METHOD FOR USE IN CROSSING A VEHICLE AND AUTONOMOUS VEHICLE TRANSPORTATION NETWORK
US20210009154A1 (en) Centralized Shared Autonomous Vehicle Operational Management
CN107664503A (en) Vehicle path planning method and device
CN108603763A (en) Traveling plan generating means, traveling scheduling method and traveling plan generate program
CN106004860A (en) Vehicle traveling control device
CN105593639B (en) Driving information storage system, method and storage medium
WO2020125686A1 (en) Method for generating real-time relative map, intelligent driving device and computer storage medium
CN105074387A (en) Travel support system, travel support method, and computer program
CN103308051A (en) Moving body position detection system, moving body position detection apparatus, moving body position detection method, and computer-readable storage medium
CN113689735A (en) Vehicle lane change collision early warning method and device
CN109598927A (en) Right of way distribution method, system, equipment and storage medium based on V2X technology
CN114384902B (en) Automatic tracking control method and system thereof
CN105009180A (en) Deceleration completion point storage system, driving assistance system, driving assistance method, and computer program
JP6268751B2 (en) Destination information providing system and method, and computer program
CN115079702A (en) Intelligent vehicle planning method and system under mixed road scene
WO2021210519A1 (en) Vehicle motion control device and vehicle motion control method
CN115230731A (en) Travel route determination method, travel route determination device, travel route determination terminal, and travel route determination medium
CN110069066A (en) A kind of method and terminal that unmanned vehicle tracks

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant