A vehicle positioning method and device in reference trajectory points

Abstract

The application discloses a kind of reference trajectory point vehicle positioning method and device, according to reference trajectory point and vehicle coordinate, the index number of the reference trajectory point closest to vehicle current position point is determined, the position information of vehicle current position point in entire reference trajectory point is determined based on the value range of index number, position information includes: reference trajectory starting point, reference trajectory termination point and reference trajectory point non-end point, in combination with vehicle current position point coordinate, corresponding cumulative reference trajectory calculation strategy is used to position information, and the cumulative reference distance of vehicle current position point in reference trajectory is obtained.The application obtains the cumulative reference distance of vehicle current position point in reference trajectory according to the different position information of vehicle current position point in entire reference trajectory point, using corresponding cumulative reference trajectory calculation strategy, so as to improve the accuracy of cumulative reference distance, so as to facilitate the accurate execution of subsequent test cases.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of intelligent driving simulation, more particularly, to a method and device for positioning a vehicle in a reference trajectory point. BACKGROUND

[0002] Currently, in the field of intelligent driving, there is a high demand for positioning a vehicle in a reference trajectory point. Positioning a vehicle in a reference trajectory point mainly involves two parameters, an index number n of a reference trajectory point closest to a current position point of the vehicle and a cumulative reference distance S of the current position point of the vehicle in the reference trajectory. v The cumulative reference distance S v refers to: assuming that there is an ideal smooth curve passing through all the reference trajectory points, the distance between the closest point of the current position point of the vehicle to the smooth curve and the starting point of the ideal smooth curve is the cumulative reference distance S v .

[0003] However, since an ideal smooth curve cannot be obtained according to all the reference trajectory points, the cumulative reference distance S v can only be estimated in the prior art. There are not many estimation algorithms for the cumulative reference distance S v , and some simplified estimation algorithms are commonly used, for example, the cumulative reference distance of the closest reference trajectory point is directly used as an approximate estimation value of S v . However, using the cumulative reference distance of the closest reference trajectory point as the approximate estimation value of S v has great limitations. If the vehicle is located between two reference trajectory points and is far away from the two reference trajectory points, there will be a large difference between the estimated cumulative reference distance S v and the true value of the cumulative reference distance. Therefore, when the index number n of the reference trajectory point and the cumulative reference distance S v are used as inputs of a distance trigger type test case in intelligent driving simulation testing, the accuracy of the execution of the test case is greatly reduced. SUMMARY

[0004] Therefore, the present application discloses a method and device for positioning a vehicle in a reference trajectory point to improve the accuracy of the cumulative reference distance, so that when the index number of the reference trajectory point and the cumulative reference distance are used as inputs of a distance trigger type test case in intelligent driving simulation testing, the test case can be accurately executed.

[0005] A method for positioning a vehicle in a reference trajectory point, comprising:

[0006] determining an index number of a reference trajectory point closest to a current position point of the vehicle according to the reference trajectory point and the coordinates of the vehicle;

[0007] determining position information of the vehicle current position point in the whole reference track point based on the value range of the index number, wherein the position information comprises a reference track starting point, a reference track ending point and a reference track point non-end point;

[0008] combining the vehicle current position point coordinate, adopting a corresponding cumulative reference track calculation strategy for the position information to obtain a cumulative reference track distance of the vehicle current position point in the reference track.

[0009] Optionally, the index number of the reference track point closest to the vehicle current position point is determined according to the reference track point and the vehicle coordinate, comprising:

[0010] judging whether the vehicle is running for the first time at the current time;

[0011] If not, determining the vehicle current position point according to the vehicle coordinate, and calculating the distance between the vehicle current position point and the nearest path point at the last time;

[0012] taking the nearest path point at the last time as a starting point, taking the distance between the vehicle current position point and the nearest path point at the last time as an initial distance, sequentially checking the change trend of the distance between the reference track point and the vehicle current position point relative to the last distance in the increasing direction of the index number until the distance becomes larger to stop checking, obtaining a forward minimum distance determined by forward checking and a forward index number of the corresponding path reference point;

[0013] taking the nearest path point at the last time as a starting point, taking the distance between the vehicle current position point and the nearest path point at the last time as an initial distance, sequentially checking the change trend of the distance between the reference track point and the vehicle current position point relative to the last distance in the decreasing direction of the index number until the distance becomes larger to stop checking, obtaining a backward minimum distance determined by backward checking and a backward index number of the corresponding path reference point;

[0014] when the forward minimum distance is not greater than the backward minimum distance, determining the forward index number as the index number of the reference track point closest to the vehicle current position point;

[0015] when the forward minimum distance is greater than the backward minimum distance, determining the backward index number as the index number of the reference track point closest to the vehicle current position point.

[0016] Optionally, the position information of the vehicle current position point in the whole reference track point is determined based on the value range of the index number, comprising:

[0017] when the index number is equal to 1, determining the nearest reference track point to the vehicle current position as a reference track starting point;

[0018] when the index number is equal to N, determining that the nearest reference trajectory point to the current position of the vehicle is a reference trajectory end point, wherein N is the total number of reference trajectory points, and N≠1;

[0019] when the index number is greater than 1 and less than N, determining that the nearest reference trajectory point to the current position of the vehicle is a reference trajectory point non-end point.

[0020] Optionally, the cumulative reference trajectory calculation strategy corresponding to the position information is adopted in combination with the coordinates of the current position point of the vehicle to obtain the cumulative reference distance of the current position point of the vehicle in the reference trajectory, comprising:

[0021] when the nearest reference trajectory point to the current position of the vehicle is a reference trajectory start point, solving a first straight line equation of a line connecting the reference trajectory start point and a second reference trajectory point;

[0022] drawing a perpendicular line corresponding to the first straight line equation through the current position point of the vehicle to obtain a first foot coordinate;

[0023] obtaining the cumulative reference distance of the current position point of the vehicle in the reference trajectory based on the first foot coordinate, the coordinates of the reference trajectory start point and the coordinates of the second reference trajectory point.

[0024] Optionally, the cumulative reference trajectory calculation strategy corresponding to the position information is adopted in combination with the coordinates of the current position point of the vehicle to obtain the cumulative reference distance of the current position point of the vehicle in the reference trajectory, comprising:

[0025] when the nearest reference trajectory point to the current position of the vehicle is a reference trajectory end point, solving a second straight line equation of a line connecting the reference trajectory end point and a second reference trajectory point;

[0026] drawing a perpendicular line corresponding to the second straight line equation through the current position point of the vehicle to obtain a second foot coordinate;

[0027] obtaining the cumulative reference distance of the current position point of the vehicle in the reference trajectory based on the second foot coordinate, the coordinates of the reference trajectory end point and the coordinates of the second reference trajectory point.

[0028] Optionally, the cumulative reference trajectory calculation strategy corresponding to the position information is adopted in combination with the coordinates of the current position point of the vehicle to obtain the cumulative reference distance of the current position point of the vehicle in the reference trajectory, comprising:

[0029] When the nearest reference trajectory point to the current position of the vehicle is a non-end point of the reference trajectory point, three adjacent reference trajectory points are selected from the reference trajectory, in order: the first reference trajectory point, the second reference trajectory point, and the third reference trajectory point;

[0030] A first median line of a line segment formed by the first reference trajectory point and the second reference trajectory point is determined.

[0031] A second median line of a line segment formed by the second reference trajectory point and the third reference trajectory point is determined.

[0032] Based on whether the first median line and the second median line have an intersection point, a different cumulative reference trajectory calculation strategy is used to obtain the cumulative reference distance of the current position point of the vehicle in the reference trajectory.

[0033] Optionally, the cumulative reference distance of the current position point of the vehicle in the reference trajectory is obtained based on whether the first median line and the second median line have an intersection point, and a different cumulative reference trajectory calculation strategy is used, including:

[0034] When the first median line and the second median line are parallel, the cumulative reference distance of the current position point of the vehicle in the reference trajectory is obtained based on a length of a line segment between the second reference trajectory point and the current position point of the vehicle, and coordinate values of the first reference trajectory point, the second reference trajectory point, and the current position point of the vehicle.

[0035] Optionally, the cumulative reference distance of the current position point of the vehicle in the reference trajectory is obtained based on whether the first median line and the second median line have an intersection point, and a different cumulative reference trajectory calculation strategy is used, including:

[0036] When the first median line and the second median line have an intersection point, the coordinate of the intersection point is determined.

[0037] It is determined whether the coordinate of the intersection point coincides with the current position point of the vehicle or the current position point of the vehicle is within a preset neighborhood of the coordinate of the intersection point.

[0038] If so, the cumulative reference distance of the current vehicle positioning point is determined as the cumulative reference distance of the current position point of the vehicle in the reference trajectory.

[0039] Optionally, the cumulative reference distance of the current position point of the vehicle in the reference trajectory is obtained based on whether the first median line and the second median line have an intersection point, and a different cumulative reference trajectory calculation strategy is used, further including:

[0040] If no, the preset range of the first and / or second median line is processed by a preset processing mode to obtain the accumulated reference distance of the current position point of the vehicle in the reference trajectory under different conditions.

[0041] A vehicle positioning device in a reference trajectory point comprises:

[0042] An index number determination unit is configured to determine an index number of a reference trajectory point closest to a current position point of a vehicle according to a reference trajectory point and a vehicle coordinate.

[0043] A position determination unit is configured to determine position information of the current position point of the vehicle in the entire reference trajectory point based on a value range of the index number, wherein the position information comprises a reference trajectory starting point, a reference trajectory ending point, and a reference trajectory point non-end point.

[0044] An accumulated reference distance determination unit is configured to obtain an accumulated reference distance of the current position point of the vehicle in the reference trajectory by adopting a corresponding accumulated reference trajectory calculation strategy for the position information in combination with a coordinate of the current position point of the vehicle.

[0045] As can be seen from the above technical solution, the present application discloses a vehicle positioning method and device in a reference trajectory point, an index number of a reference trajectory point closest to a current position point of a vehicle is determined according to a reference trajectory point and a vehicle coordinate, position information of the current position point of the vehicle in the entire reference trajectory point is determined based on a value range of the index number, the position information comprises a reference trajectory starting point, a reference trajectory ending point, and a reference trajectory point non-end point, an accumulated reference distance of the current position point of the vehicle in the reference trajectory is obtained by adopting a corresponding accumulated reference trajectory calculation strategy for the position information in combination with a coordinate of the current position point of the vehicle. As can be seen from the above, the present application adopts a corresponding accumulated reference trajectory calculation strategy to obtain an accumulated reference distance of the current position point of the vehicle in the reference trajectory according to different position information of the current position point of the vehicle in the entire reference trajectory point, which greatly improves the accuracy of the accumulated reference distance compared with the traditional scheme of directly taking the accumulated reference trajectory of the nearest reference trajectory point as an approximate estimated value, and thus, when the index number of the reference trajectory point and the accumulated reference trajectory are taken as inputs of a distance trigger type test case in intelligent driving simulation testing, the test case can be accurately executed. BRIEF DESCRIPTION OF DRAWINGS

[0046] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the drawings needed to be used in the embodiments or prior art description will be briefly introduced below. Obviously, the drawings in the following description are only embodiments of the present application, and other drawings can be obtained by those skilled in the art without creative effort based on the disclosed drawings.

[0047] Figure 1 A reference trajectory point vehicle positioning method flow chart disclosed by the embodiment of the present application;

[0048] Figure 2 A method flow chart for determining the index number of the reference trajectory point closest to the current vehicle position point according to the reference trajectory point and the vehicle coordinate disclosed by the embodiment of the present application;

[0049] Figure 3 A schematic diagram for calculating the vehicle positioning according to the reference trajectory point and the vehicle coordinate point disclosed by the embodiment of the present application;

[0050] Figure 4 A structure schematic diagram of a reference trajectory point vehicle positioning device disclosed by the embodiment of the present application. DETAILED DESCRIPTION

[0051] The technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative work fall within the scope of protection of the present application.

[0052] The embodiment of the present application discloses a reference trajectory point vehicle positioning method and device, determines the index number of the reference trajectory point closest to the current vehicle position point according to the reference trajectory point and the vehicle coordinate, determines the position information of the current vehicle position point in the entire reference trajectory point based on the value range of the index number, the position information includes: the reference trajectory starting point, the reference trajectory ending point and the reference trajectory point non-end point, in combination with the vehicle current position point coordinate, adopts the corresponding cumulative reference trajectory calculation strategy for the position information to obtain the cumulative reference distance of the current vehicle position point in the reference trajectory. As can be seen from this, the present application adopts the corresponding cumulative reference trajectory calculation strategy to obtain the cumulative reference distance of the current vehicle position point in the reference trajectory according to the different position information of the current vehicle position point in the entire reference trajectory point, which greatly improves the accuracy of the cumulative reference distance compared with the traditional scheme which directly takes the cumulative reference trajectory of the nearest reference trajectory point as the approximate estimated value. Therefore, when the index number of the reference trajectory point and the cumulative reference trajectory are taken as the input of the distance trigger type test case in intelligent driving simulation test, the accurate execution of the test case is facilitated.

[0053] Referring to Figure 1 The embodiment of the present application discloses a reference trajectory point vehicle positioning method flow chart, which comprises:

[0054] Step S101, determining the index number of the reference trajectory point closest to the current vehicle position point according to the reference trajectory point and the vehicle coordinate;

[0055] Step S102, based on the value range of the index number, determine the position information of the vehicle current position point in the entire reference trajectory point;

[0056] Wherein, the position information includes: the front of the entire reference trajectory point, the back of the entire reference trajectory point and the middle of the entire reference trajectory point.

[0057] It should be noted that when the cumulative reference distance S v of the vehicle current position point in the reference trajectory is solved, it is first necessary to determine whether the index number determined in step S101 is the reference trajectory starting point or the reference trajectory ending point.

[0058] Specifically, when the index number n = 1, the nearest reference trajectory point to the vehicle current position is determined as the reference trajectory starting point;

[0059] When the index number n = N, the nearest reference trajectory point to the vehicle current position is determined as the reference trajectory ending point, wherein N is the total number of reference trajectory points, and N ≠ 1;

[0060] When 1 < index number n < N, the nearest reference trajectory point to the vehicle current position is determined as the non-end point of the reference trajectory point.

[0061] The non-end point of the reference trajectory point in the embodiment refers to the reference trajectory point in all reference trajectory points except the reference trajectory starting point and the reference trajectory ending point.

[0062] Step S103, in combination with the vehicle current position point coordinates, the corresponding cumulative reference trajectory calculation strategy is adopted for the position information to obtain the cumulative reference distance of the vehicle current position point in the reference trajectory.

[0063] In the embodiment, different cumulative reference trajectory calculation strategies are adopted for different position information of the vehicle current position point in the entire reference trajectory point, so as to improve the accuracy of the cumulative reference trajectory calculation result.

[0064] In conclusion, the present application discloses a kind of reference trajectory point vehicle positioning method, the index number of reference trajectory point closest to vehicle current position point is determined according to reference trajectory point and vehicle coordinates, the position information of vehicle current position point in entire reference trajectory point is determined based on the value range of index number, position information includes: reference trajectory starting point, reference trajectory end point and reference trajectory point non-end point, in combination with vehicle current position point coordinates, corresponding cumulative reference trajectory calculation strategy is used for position information, and the cumulative reference distance of vehicle current position point in reference trajectory is obtained.It can be seen from this that, according to the different position information of vehicle current position point in entire reference trajectory point, the cumulative reference distance of vehicle current position point in reference trajectory is obtained using corresponding cumulative reference trajectory calculation strategy, compared with the approximate estimated value of the cumulative reference trajectory of the nearest reference trajectory point in the traditional scheme, the accuracy of cumulative reference distance is greatly improved, therefore, when the index number of reference trajectory point and cumulative reference trajectory are used as the input of distance trigger type test case in intelligent driving simulation test, the accurate execution of test case is facilitated.

[0065] To further optimize the above embodiment, refer to Figure 2 The embodiment of the present application discloses a method flow chart for determining the index number of the reference trajectory point closest to the vehicle current position point according to the reference trajectory point and the vehicle coordinates, i.e., step S101 can specifically include:

[0066] Step S201, judge whether the vehicle is running for the first time at the current time, if not, execute step S202;

[0067] When the vehicle is running for the first time at the current time, the target reference trajectory point closest to the vehicle current position point can be directly found by traversal method or dichotomy, and the index number n(0) of the target reference trajectory point is determined as the index number of the reference trajectory point closest to the vehicle current position point.

[0068] Step S202, determine the vehicle current position point according to vehicle coordinates, and calculate the distance between the vehicle current position point and the nearest path point at the last time;

[0069] When the vehicle is not running for the first time at the current time, first calculate the distance between the vehicle current position point and the nearest path point at the last time, and the last time in the embodiment refers to the last time of the current time.

[0070] The index number of the nearest path point at the last time is n(k-1), and k represents the current time, so the index number of the reference trajectory point closest to the vehicle current position point at the current time is n(k).

[0071] Step S203, based on the distance between the current position point of the vehicle and the nearest path point at the last time, obtaining the forward minimum distance determined by forward checking and the forward index number of the corresponding path reference point;

[0072] Specifically, taking the nearest path point at the last time as the starting point, taking the distance between the current position point of the vehicle and the nearest path point at the last time as the initial distance, sequentially checking the change trend of the distance between the reference trajectory point and the current position point of the vehicle relative to the last distance in the direction of increasing index number, until the distance becomes larger, stopping checking, obtaining the forward minimum distance determined by forward checking and the forward index number of the corresponding path reference point.

[0073] That is, taking the nearest path point at the last time with index number n(k-1) as the starting point, sequentially checking the distance between the current reference trajectory point and the current position point of the vehicle in the direction of increasing index number, whether the distance between the last reference trajectory point and the current position point of the vehicle is increasing or decreasing, if the distance is decreasing, continue to check forward, until the distance becomes larger, stopping checking, obtaining the forward minimum distance S min,f and the forward index number n f (k) of the corresponding path reference point determined by forward checking.

[0074] Step S204, based on the distance between the current position point of the vehicle and the nearest path point at the last time, obtaining the backward minimum distance determined by backward checking and the backward index number of the corresponding path reference point;

[0075] Specifically, taking the nearest path point at the last time as the starting point, taking the distance between the current position point of the vehicle and the nearest path point at the last time as the initial distance, sequentially checking the change trend of the distance between the reference trajectory point and the current position point of the vehicle relative to the last distance in the direction of decreasing index number, until the distance becomes larger, stopping checking, obtaining the backward minimum distance determined by backward checking and the backward index number of the corresponding path reference point.

[0076] That is, taking the nearest path point at the last time with index number n(k-1) as the starting point, sequentially checking the distance between the reference trajectory point and the current position point of the vehicle in the direction of decreasing index number, whether the distance between the last reference trajectory point and the current position point of the vehicle is increasing or decreasing, if the distance is decreasing, continue to check forward, until the distance becomes larger, stopping checking, obtaining the backward minimum distance S min,r and the backward index number n r (k) of the corresponding path reference point determined by backward checking.

[0077] Step S205, when the forward minimum distance is not greater than the backward minimum distance, determining the forward index number as the index number of the reference trajectory point closest to the current position point of the vehicle;

[0078] Comparing the forward minimum distance S min,f and the backward minimum distance Smin,r , if S min,f ≤ S min,r , then the index number n(k) of the reference trajectory point closest to the current position point of the vehicle is n f (k).

[0079] Step S206, when the current forward minimum distance is greater than the current backward minimum distance, the backward index number is determined as the index number of the reference trajectory point closest to the current position point of the vehicle.

[0080] , if S min,f ≤ S min,r , then the index number n(k) of the reference trajectory point closest to the current position point of the vehicle is n r (k).

[0081] It should be particularly pointed out that in step S103, different cumulative reference trajectory calculation strategies are adopted according to whether the closest reference trajectory point to the current position of the vehicle is the reference trajectory starting point, the reference trajectory ending point or a non-end point of the reference trajectory point. The specific strategies are as follows:

[0082] (1) The closest reference trajectory point to the current position of the vehicle is the reference trajectory starting point.

[0083] When the closest reference trajectory point to the current position of the vehicle is the reference trajectory starting point, the first straight line equation of the line connecting the reference trajectory starting point and the second reference trajectory point is solved.

[0084] A perpendicular line corresponding to the first straight line equation is drawn through the current position point of the vehicle to obtain the first foot coordinates.

[0085] Based on the first foot coordinates, the coordinates of the reference trajectory starting point and the coordinates of the second reference trajectory point, the cumulative reference distance of the current position point of the vehicle in the reference trajectory is obtained.

[0086] Wherein, the reference trajectory starting point is taken as the first reference trajectory point, and the second reference trajectory point refers to the second reference trajectory point starting from the reference trajectory starting point.

[0087] Specifically, the first straight line equation of the line connecting the reference trajectory starting point and the second reference trajectory point is solved as follows:

[0088] ax+by+c=0;

[0089] Assuming that the coordinates of the reference trajectory starting point are (X1, Y1), the coordinates of the second reference trajectory point are (X2, Y2), and the coordinates of the current position point of the vehicle are (X v ,Y v ), the coefficients of the first straight line equation are:

[0090] If Otherwise,

[0091] Draw a perpendicular line through the current position point of the vehicle, which is perpendicular to the straight line corresponding to the first straight line equation, to obtain the first perpendicular foot coordinate as follows:

[0092]

[0093] At this time, the cumulative reference distance S of the current position point of the vehicle in the reference trajectory v is estimated as follows:

[0094]

[0095] In the formula, S1 represents the cumulative reference distance of the first reference trajectory point, S2 represents the cumulative reference distance of the second reference trajectory point, and sgn is a symbol taking symbol, which is a general mathematical symbol, such as sgn(-3.234)=-1.

[0096] (2) The nearest reference trajectory point to the current position of the vehicle is the reference trajectory termination point;

[0097] When the nearest reference trajectory point to the current position of the vehicle is the reference trajectory termination point, the second straight line equation of the line connecting the reference trajectory termination point and the second last reference trajectory point is solved;

[0098] Draw a perpendicular line through the current position point of the vehicle, which is perpendicular to the straight line corresponding to the second straight line equation, to obtain the second perpendicular foot coordinate;

[0099] Based on the second perpendicular foot coordinate, the coordinates of the reference trajectory termination point and the coordinates of the second last reference trajectory point, the cumulative reference distance of the current position point of the vehicle in the reference trajectory is obtained.

[0100] Among them, the reference trajectory termination point is taken as the first reference trajectory point, and the second last reference trajectory point refers to the second reference trajectory point starting from the reference trajectory termination point.

[0101] Specifically, the second straight line equation of the line connecting the reference trajectory termination point and the second last reference trajectory point is solved as follows:

[0102] ax+by+c=0;

[0103] Suppose the coordinates of the reference trajectory termination point are (X N , Y N ), the coordinates of the second last reference trajectory point are (X N-1 , Y N-1 ), and the coordinates of the current position point of the vehicle are (X v , Y v ), then the coefficients of the second straight line equation are as follows:

[0104]

[0105] A perpendicular line corresponding to the straight line of the second straight line equation is drawn through the current position point of the vehicle, and the second foot coordinate is obtained as follows:

[0106]

[0107] At this time, the cumulative reference distance S of the current position point of the vehicle in the reference trajectory v is estimated as follows:

[0108]

[0109] In the formula, S N-1 represents the cumulative reference distance of the last reference trajectory point, and S N represents the cumulative reference distance of the second last reference trajectory point.

[0110] (3) The nearest reference trajectory point to the current position of the vehicle is a reference trajectory point that is not an end point;

[0111] When the nearest reference trajectory point to the current position of the vehicle is a reference trajectory point that is not an end point, three adjacent reference trajectory points are selected from the reference trajectory, in order: the first reference trajectory point, the second reference trajectory point, and the third reference trajectory point;

[0112] The first median of the line segment formed by the first reference trajectory point and the second reference trajectory point is determined;

[0113] The second median of the line segment formed by the second reference trajectory point and the third reference trajectory point is determined;

[0114] Based on whether the first median and the second median have an intersection point, different cumulative reference trajectory calculation strategies are used to obtain the cumulative reference distance of the current position point of the vehicle in the reference trajectory.

[0115] Specifically, see Figure 3The schematic diagram for calculating the vehicle positioning according to the reference track point and the vehicle coordinate point is shown, the vehicle coordinate point V (Xv, Yv, Sv), the point O (Xo, Yo) is the vehicle reference track point, the point A (Xa, Ya) and the point B (Xb, Yb) are respectively the previous point and the next point of the vehicle reference track point O, the point M1 (Xm1, Ym1) and the point M2 (Xm2, Ym2) are respectively the midpoints of OA and OB, PA and PB are respectively the perpendicular bisectors of OA and OB, the two perpendicular bisectors intersect at the point P (Xp, Yp). V is the current position of the vehicle, the straight line PV intersects with the OA straight line at the point Q1 (Xq1, Yq1), the straight line PV intersects with the OB straight line at the point Q2 (Xq2, Yq2), the point 1 (X1, Y1, S1), the point 2 (X2, Y2, S2), the point 3 (X3, Y3, S3), the point 1, the point 2 and the point 3 represent the previous three reference track points. It is assumed that the selected reference track adjacent three points A (X n-1 ,Y n-1 ), O (X n ,Y n ) and B (X n+1 ,Y n+1 ), the current position of the vehicle is V (X v ,Y v ), wherein the current position of the vehicle is generally the center point of the front axle of the vehicle.

[0116] Supposing that the perpendicular bisector equation coefficients of the line segment OA and the line segment OB are a z1 ,b z1 ,c z1 and a z2 ,b z2 ,c z2 respectively, then the normalized perpendicular bisector equation coefficients are as follows:

[0117] If (Y n =Y n-1 ), otherwise, if (X n =X n-1 ), otherwise,

[0118]

[0119] If (Y n+1 =Y n ), otherwise, if (X n+1 =X n ), otherwise,

[0120]

[0121] In actual application, the line segment OA and the perpendicular bisector and the perpendicular bisector of the line segment OB exist in parallel and intersection, the present application adopts different cumulative reference track calculation strategies for the two cases, which are as follows:

[0122] 1) When the first and second median lines are parallel, the cumulative reference distance of the current vehicle position point in the reference trajectory is obtained based on the length of the line segment between the second reference trajectory point and the current vehicle position point, and the coordinate values of the first reference trajectory point, the second reference trajectory point, and the current vehicle position point.

[0123] Specifically, if the first and second median lines are parallel and have no intersection point, ∠VOA = θ1 and ∠VOB = θ2, which can be calculated based on the coordinate values of the four points A, O, B, and the current vehicle position point V in the reference trajectory.

[0124] The cumulative reference distance S of the current vehicle position point in the reference trajectory v is estimated as follows:

[0125]

[0126] In the formula, S n represents the cumulative reference distance of the current positioning point, i.e., the cumulative reference distance of point O, r OV represents the length of line segment OV.

[0127] 2) When the first and second median lines have an intersection point, the coordinates of the intersection point are determined.

[0128] It is determined whether the coordinates of the intersection point coincide with the current vehicle position point or are within a preset neighborhood of the current vehicle position point.

[0129] If yes, the cumulative reference distance of the current vehicle positioning point is determined as the cumulative reference distance of the current vehicle position point in the reference trajectory.

[0130] Specifically, if the two median lines are not parallel and have an intersection point, the coordinates of the intersection point are denoted as P(X p , Y p ).

[0131] The coordinates of the intersection point of the first and second median lines are solved as follows:

[0132]

[0133] If the intersection point P coincides with the current vehicle position point V or the current vehicle position point V is within a very small neighborhood of the intersection point P, i.e., |r OV -r OP |≤ε, r OV represents the length of line segment OV, r OP represents the length of line segment OP, and ε represents a minimum critical difference value, which is very small, such as 0.0001, the cumulative reference distance S v of the current vehicle position point in the reference trajectory is estimated as follows:

[0134] S v =S n .

[0135] Among them, S n This indicates the cumulative reference distance at the current vehicle location point (i.e., point O).

[0136] If the intersection point P does not coincide with the vehicle's current position point V, then the preset range of the first perpendicular bisector and / or the second perpendicular bisector is processed using a preset processing method to obtain the cumulative reference distance of the vehicle's current position point under different conditions in the reference trajectory, as follows:

[0137] When |r OV -r OP When |>ε, meaning the intersection point P does not coincide with the vehicle's current position point V, to avoid boundary judgment loopholes near the perpendicular bisector of line segment OA or OB, a preset processing method is used within the preset range of the first perpendicular bisector and / or the second perpendicular bisector. The preset processing method is as follows:

[0138]

[0139] In the formula, r VA r represents the length of line segment VA. VO r represents the length of line segment VO. VB This represents the length of line segment VB.

[0140] The main task below is to solve S. x S x Let Sv be the expression of a piecewise function under certain conditions. The equation of the line connecting PV is:

[0141] like otherwise,

[0142] Let the intersection points of line PV with line segments OA and OB be Q1(X) and Q2(X). q1 ,Y q1 ) and Q2(X q2 ,Y q2 ),but:

[0143]

[0144]

[0145] The midpoints of line segments OA and OB are M1(X) and M2(OB), respectively. m1 ,Y m1 ) and M2(X m2 ,Y m2 ),but:

[0146]

[0147] The logic condition for judging whether the intersection point Q1 (X q1 ,Y q1 ) is inside the line segment M1O is:

[0148] (X n -X q1 )(X q1 -X m1 )+(Y n -Y q1 )(Y q1 -Y m1 )≥0 (condition C1)

[0149] The logic condition for judging whether the intersection point Q2 (X q2 ,Y q2 ) is inside the line segment OM2 is:

[0150] (X n -X q2 )(X q2 -X m2 )+(Y n -Y q2 )(Y q2 -Y m2 )≥0 (condition C2)

[0151] In theory, at least one of the conditions C1 and C2 is true, then S x The estimation formula of S x includes the following three cases:

[0152] Case One

[0153] If both the condition C1 and the condition C2 are true, then:

[0154] S x =S n ;

[0155] Case Two

[0156] If only the condition C1 is true and the condition C2 is not true, then:

[0157]

[0158] Case Three

[0159] If only the condition C2 is true and the condition C1 is not true, then:

[0160]

[0161] Corresponding to the above method embodiment, the application further discloses a vehicle positioning device with reference to a track point.

[0162] Referring to Figure 4 The embodiment of the present application discloses a structural diagram of a vehicle positioning device in reference trajectory points, which comprises:

[0163] An index number determination unit 301 is configured to determine an index number of a reference trajectory point closest to a current position point of a vehicle according to reference trajectory points and vehicle coordinates;

[0164] A position determination unit 302 is configured to determine position information of the current position point of the vehicle in the entire reference trajectory points based on a value range of the index number;

[0165] The position information comprises a reference trajectory starting point, a reference trajectory ending point and a reference trajectory point non-end point.

[0166] It should be noted that when the cumulative reference distance S v of the current position point of the vehicle in the reference trajectory is solved, it is necessary to first determine whether the index number is the reference trajectory starting point or the reference trajectory ending point.

[0167] A cumulative reference distance determination unit 303 is configured to obtain the cumulative reference distance of the current position point of the vehicle in the reference trajectory by adopting a corresponding cumulative reference trajectory calculation strategy for the position information in combination with the coordinates of the current position point of the vehicle.

[0168] In the embodiment, different cumulative reference trajectory calculation strategies are adopted for different position information of the current position point of the vehicle in the entire reference trajectory points, so as to improve the accuracy of the cumulative reference trajectory calculation result.

[0169] As can be seen from the above, the present application discloses a vehicle positioning device in reference trajectory points, which determines an index number of a reference trajectory point closest to a current position point of a vehicle according to reference trajectory points and vehicle coordinates, determines position information of the current position point of the vehicle in the entire reference trajectory points based on a value range of the index number, adopts a corresponding cumulative reference trajectory calculation strategy for the position information in combination with the coordinates of the current position point of the vehicle, and obtains the cumulative reference distance of the current position point of the vehicle in the reference trajectory. It can be seen that the present application adopts a corresponding cumulative reference trajectory calculation strategy for different position information of the current position point of the vehicle in the entire reference trajectory points to obtain the cumulative reference distance of the current position point of the vehicle in the reference trajectory. Compared with the traditional scheme which directly takes the cumulative reference trajectory of the closest reference trajectory point as an approximate estimation value, the accuracy of the cumulative reference distance is greatly improved. Therefore, when the index number of the reference trajectory point and the cumulative reference trajectory are taken as inputs of a distance trigger type test case in intelligent driving simulation testing, the test case can be accurately executed.

[0170] To further optimize the above embodiment, the index number determination unit 301 is specifically configured to:

[0171] determine whether the vehicle is running for the first time at the current time;

[0172] If not, determine the current position point of the vehicle according to the vehicle coordinates, and calculate the distance between the current position point of the vehicle and the nearest path point at the last time;

[0173] Take the nearest path point at the last time as the starting point, and take the distance between the current position point of the vehicle and the nearest path point at the last time as the initial distance, and sequentially check the change trend of the distance between the reference trajectory point and the current position point of the vehicle relative to the last distance in the increasing direction of the index number, until the distance becomes larger, stop checking, and obtain the forward minimum distance determined by the forward checking and the forward index number of the corresponding path reference point;

[0174] Take the nearest path point at the last time as the starting point, and take the distance between the current position point of the vehicle and the nearest path point at the last time as the initial distance, and sequentially check the change trend of the distance between the reference trajectory point and the current position point of the vehicle relative to the last distance in the decreasing direction of the index number, until the distance becomes larger, stop checking, and obtain the backward minimum distance determined by the backward checking and the backward index number of the corresponding path reference point;

[0175] When the forward minimum distance is not greater than the backward minimum distance, the forward index number is determined as the index number of the reference trajectory point closest to the current position point of the vehicle;

[0176] When the forward minimum distance is greater than the backward minimum distance, the backward index number is determined as the index number of the reference trajectory point closest to the current position point of the vehicle.

[0177] To further optimize the above embodiment, the position determination unit 302 is specifically configured to:

[0178] When the index number is equal to 1, determine that the nearest reference trajectory point to the current position of the vehicle is the reference trajectory starting point;

[0179] When the index number is equal to N, determine that the nearest reference trajectory point to the current position of the vehicle is the reference trajectory termination point, wherein N is the total number of reference trajectory points, and N≠1;

[0180] When the index number is greater than 1 and less than N, determine that the nearest reference trajectory point to the current position of the vehicle is a non-end point of the reference trajectory point.

[0181] To further optimize the above embodiment, the cumulative reference distance determination unit 303 can specifically include:

[0182] a first solving subunit, configured to solve a first straight line equation of a line connecting the reference trajectory start point and a second reference trajectory point when the nearest reference trajectory point to the current vehicle position is the reference trajectory start point;

[0183] a first foot point coordinate determining subunit, configured to draw a perpendicular line vertical to the straight line corresponding to the first straight line equation through the current vehicle position point to obtain a first foot point coordinate;

[0184] a first cumulative reference distance determining subunit, configured to obtain a cumulative reference distance of the current vehicle position point in the reference trajectory based on the first foot point coordinate, a coordinate of the reference trajectory start point and a coordinate of the second reference trajectory point.

[0185] To further optimize the above embodiment, the cumulative reference distance determining unit 303 can specifically include:

[0186] a second solving subunit, configured to solve a second straight line equation of a line connecting the reference trajectory end point and a penultimate reference trajectory point when the nearest reference trajectory point to the current vehicle position is the reference trajectory end point;

[0187] a second foot point coordinate determining subunit, configured to draw a perpendicular line vertical to the straight line corresponding to the second straight line equation through the current vehicle position point to obtain a second foot point coordinate;

[0188] a second cumulative reference distance determining subunit, configured to obtain a cumulative reference distance of the current vehicle position point in the reference trajectory based on the second foot point coordinate, a coordinate of the reference trajectory end point and a coordinate of the penultimate reference trajectory point.

[0189] To further optimize the above embodiment, the cumulative reference distance determining unit 303 can specifically include:

[0190] a trajectory point selecting subunit, configured to select three adjacent reference trajectory points from the reference trajectory when the nearest reference trajectory point to the current vehicle position is a non-end point of the reference trajectory point, and the three adjacent reference trajectory points are sequentially a first reference trajectory point, a second reference trajectory point and a third reference trajectory point;

[0191] a first mid-perpendicular line determining subunit, configured to determine a first mid-perpendicular line of a line segment formed by the first reference trajectory point and the second reference trajectory point;

[0192] a second mid-perpendicular line determining subunit, configured to determine a second mid-perpendicular line of a line segment formed by the second reference trajectory point and the third reference trajectory point;

[0193] a third cumulative reference distance determining subunit, configured to obtain a cumulative reference distance of the current vehicle position point in the reference trajectory by using different cumulative reference trajectory calculation strategies based on whether the first mid-perpendicular line and the second mid-perpendicular line have an intersection point.

[0194] To further optimize the above embodiment, the third cumulative reference distance determining sub-unit can be specifically used for:

[0195] When the first median line and the second median line are parallel, the cumulative reference distance of the vehicle current position point in the reference trajectory is obtained based on the length of the line segment between the second reference trajectory point and the vehicle current position point, and the coordinate values of the first reference trajectory point, the second reference trajectory point and the vehicle current position point.

[0196] To further optimize the above embodiment, the third cumulative reference distance determining sub-unit can be specifically used for:

[0197] When the first median line and the second median line have an intersection point, the intersection point coordinate is determined.

[0198] It is judged whether the intersection point coordinate coincides with the vehicle current position point or the vehicle current position point is in a preset neighborhood of the intersection point coordinate.

[0199] If yes, the cumulative reference distance of the current vehicle positioning point is determined as the cumulative reference distance of the vehicle current position point in the reference trajectory.

[0200] If no, the preset range of the first median line and / or the second median line is processed by using a preset processing mode to obtain the cumulative reference distance of the vehicle current position point in the reference trajectory under different conditions.

[0201] It should be particularly noted that the specific working principles of the components in the device embodiment can be referred to the corresponding parts of the method embodiment, which will not be repeated here.

[0202] Finally, it should be noted that in this document, relationship terms such as first and second are used only to distinguish one entity or operation from another, and do not necessarily require or imply that there is any such actual relationship or order between these entities or operations. Moreover, the terms "include", "contain" or any other variants thereof are intended to cover non-exclusive inclusion, so that the process, method, article or equipment including a series of elements not only includes those elements, but also includes other elements not explicitly listed or inherent to such process, method, article or equipment. Without more limitations, the element defined by the statement "including a" does not exclude the presence of other identical elements in the process, method, article or equipment including the element.

[0203] Each embodiment in the specification is described in a progressive manner, and each embodiment focuses on the difference from other embodiments. The same or similar parts between each embodiment can be referred to each other.

[0204] The foregoing description of the disclosed embodiments enables a person skilled in the art to make or use the application. Modifications of these embodiments will occur to persons of skill in the art, and that the appended claims are intended to cover all such modifications that do not depart from the true spirit and scope of the application. Therefore, the application is not limited to the embodiments shown but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A method for locating a vehicle in a reference trajectory point, characterized in that, include: Determine the index number of the reference trajectory point closest to the vehicle's current position based on the reference trajectory point and the vehicle coordinates; Based on the range of values ​​for the index number, the position information of the vehicle's current location point within the entire reference trajectory point is determined, wherein the position information includes: the reference trajectory start point, the reference trajectory end point, and the non-endpoints of the reference trajectory point; By combining the vehicle's current location coordinates, a corresponding cumulative reference trajectory calculation strategy is adopted for the location information to obtain the cumulative reference distance of the vehicle's current location in the reference trajectory; The step of combining the vehicle's current location coordinates and employing a corresponding cumulative reference trajectory calculation strategy to obtain the cumulative reference distance of the vehicle's current location in the reference trajectory includes: When the nearest reference trajectory point to the current position of the vehicle is the starting point of the reference trajectory, solve the equation of the first straight line connecting the starting point of the reference trajectory and the second positive reference trajectory point; draw a perpendicular line through the current position of the vehicle to the straight line corresponding to the first straight line equation to obtain the coordinates of the first perpendicular foot; based on the coordinates of the first perpendicular foot, the coordinates of the starting point of the reference trajectory, and the coordinates of the second positive reference trajectory point, obtain the cumulative reference distance of the current position of the vehicle in the reference trajectory; When the nearest reference trajectory point to the vehicle's current position is the reference trajectory termination point, solve for the equation of the second straight line connecting the reference trajectory termination point and the second-to-last reference trajectory point; draw a perpendicular line through the vehicle's current position point to the line corresponding to the second straight line equation to obtain the coordinates of the second perpendicular foot; based on the coordinates of the second perpendicular foot, the coordinates of the reference trajectory termination point, and the coordinates of the second-to-last reference trajectory point, obtain the cumulative reference distance of the vehicle's current position point in the reference trajectory; When the nearest reference trajectory point to the vehicle's current position is not an endpoint of the reference trajectory point, three adjacent reference trajectory points are arbitrarily selected from the reference trajectory, namely: the first reference trajectory point, the second reference trajectory point, and the third reference trajectory point; the first perpendicular bisector of the line segment formed by the first reference trajectory point and the second reference trajectory point is determined; the second perpendicular bisector of the line segment formed by the second reference trajectory point and the third reference trajectory point is determined; based on whether the first perpendicular bisector and the second perpendicular bisector intersect, different cumulative reference trajectory calculation strategies are used to obtain the cumulative reference distance of the vehicle's current position point in the reference trajectory.

2. The vehicle positioning method in reference trajectory points according to claim 1, characterized in that, The step of determining the index number of the reference trajectory point closest to the vehicle's current position based on the reference trajectory point and the vehicle coordinates includes: Determine whether this is the vehicle's first run at this moment; If not, determine the current location of the vehicle based on the vehicle coordinates, and calculate the distance between the current location of the vehicle and the nearest path point at the previous moment; Starting from the nearest path point at the previous moment, and taking the distance between the current position of the vehicle and the nearest path point at the previous moment as the initial distance, the distance between the reference trajectory point and the current position of the vehicle relative to the previous distance is checked sequentially in the direction of increasing index number until the distance increases and the check is stopped, thus obtaining the minimum forward distance determined by forward check and the forward index number of the corresponding path reference point. Starting from the nearest path point at the previous moment, and taking the distance between the current position of the vehicle and the nearest path point at the previous moment as the initial distance, the changing trend of the distance between the reference trajectory point and the current position of the vehicle relative to the previous distance is checked sequentially in the direction of decreasing index number, until the distance increases and the check is stopped, thus obtaining the backward minimum distance determined by backward check and the backward index number of the corresponding path reference point; When the forward minimum distance is not greater than the backward minimum distance, the forward index number is determined to be the index number of the reference trajectory point closest to the current position of the vehicle; When the forward minimum distance is greater than the backward minimum distance, the backward index number is determined to be the index number of the reference trajectory point closest to the current position of the vehicle.

3. The vehicle positioning method in reference trajectory points according to claim 1, characterized in that, Determining the position information of the vehicle's current location within the entire reference trajectory points based on the value range of the index number includes: When the index number is equal to 1, the nearest reference trajectory point to the current position of the vehicle is determined as the starting point of the reference trajectory; When the index number is equal to N, the nearest reference trajectory point to the current position of the vehicle is determined as the reference trajectory termination point, where N is the total number of reference trajectory points and N≠1; When the index number is greater than 1 and less than N, the nearest reference trajectory point to the current position of the vehicle is determined as a non-endpoint of the reference trajectory point.

4. The vehicle positioning method in reference trajectory points according to claim 1, characterized in that, The step of obtaining the cumulative reference distance of the vehicle's current position in the reference trajectory by employing different cumulative reference trajectory calculation strategies based on whether the first perpendicular bisector and the second perpendicular bisector intersect includes: When the first perpendicular bisector and the second perpendicular bisector are parallel, the cumulative reference distance of the vehicle's current position point in the reference trajectory is obtained based on the line segment length between the second reference trajectory point and the vehicle's current position point, as well as the coordinate values ​​of the first reference trajectory point, the second reference trajectory point, and the vehicle's current position point.

5. The vehicle positioning method in reference trajectory points according to claim 1, characterized in that, The step of obtaining the cumulative reference distance of the vehicle's current position in the reference trajectory by employing different cumulative reference trajectory calculation strategies based on whether the first perpendicular bisector and the second perpendicular bisector intersect includes: When the first perpendicular bisector and the second perpendicular bisector intersect, determine the coordinates of the intersection point; Determine whether the coordinates of the intersection point coincide with the current position of the vehicle or whether the current position of the vehicle is within a preset neighborhood of the coordinates of the intersection point; If so, the cumulative reference distance of the current vehicle location point is determined as the cumulative reference distance of the current vehicle location point in the reference trajectory.

6. The vehicle positioning method in reference trajectory points according to claim 5, characterized in that, The step of obtaining the cumulative reference distance of the vehicle's current position point in the reference trajectory based on whether the first perpendicular bisector and the second perpendicular bisector intersect, using different cumulative reference trajectory calculation strategies, further includes: If not, then the preset range of the first perpendicular bisector and / or the second perpendicular bisector is processed using a preset processing method to obtain the cumulative reference distance of the vehicle's current position point in the reference trajectory under different conditions.

7. A vehicle positioning device based on a reference trajectory point, characterized in that, include: The index number determination unit is used to determine the index number of the reference trajectory point closest to the vehicle's current position point based on the reference trajectory point and the vehicle coordinates. A position determination unit is used to determine the position information of the vehicle's current position point in the entire reference trajectory point based on the value range of the index number, wherein the position information includes: the reference trajectory start point, the reference trajectory end point, and the non-endpoints of the reference trajectory point; The cumulative reference distance determination unit is used to combine the coordinates of the vehicle's current position point and adopt a corresponding cumulative reference trajectory calculation strategy for the position information to obtain the cumulative reference distance of the vehicle's current position point in the reference trajectory. The step of combining the vehicle's current location coordinates and employing a corresponding cumulative reference trajectory calculation strategy to obtain the cumulative reference distance of the vehicle's current location in the reference trajectory includes: When the nearest reference trajectory point to the current position of the vehicle is the starting point of the reference trajectory, solve the equation of the first straight line connecting the starting point of the reference trajectory and the second positive reference trajectory point; draw a perpendicular line through the current position of the vehicle to the straight line corresponding to the first straight line equation to obtain the coordinates of the first perpendicular foot; based on the coordinates of the first perpendicular foot, the coordinates of the starting point of the reference trajectory, and the coordinates of the second positive reference trajectory point, obtain the cumulative reference distance of the current position of the vehicle in the reference trajectory; When the nearest reference trajectory point to the vehicle's current position is the reference trajectory termination point, solve for the equation of the second straight line connecting the reference trajectory termination point and the second-to-last reference trajectory point; draw a perpendicular line through the vehicle's current position point to the line corresponding to the second straight line equation to obtain the coordinates of the second perpendicular foot; based on the coordinates of the second perpendicular foot, the coordinates of the reference trajectory termination point, and the coordinates of the second-to-last reference trajectory point, obtain the cumulative reference distance of the vehicle's current position point in the reference trajectory; When the nearest reference trajectory point to the vehicle's current position is not an endpoint of the reference trajectory point, three adjacent reference trajectory points are arbitrarily selected from the reference trajectory, namely: the first reference trajectory point, the second reference trajectory point, and the third reference trajectory point; the first perpendicular bisector of the line segment formed by the first reference trajectory point and the second reference trajectory point is determined; the second perpendicular bisector of the line segment formed by the second reference trajectory point and the third reference trajectory point is determined; based on whether the first perpendicular bisector and the second perpendicular bisector intersect, different cumulative reference trajectory calculation strategies are used to obtain the cumulative reference distance of the vehicle's current position point in the reference trajectory.

Images