Pose point calculation method of large-radian arc curve path and storage medium

Abstract

The invention discloses a pose point calculation method of a large-radian arc curve path and a storage medium, and the method specifically comprises the following steps: 1, judging whether a vehicle advances or retreats in real time, if the vehicle advances, entering a step 2, and if the vehicle retreats, entering a step 3; 2, in the advancing process of the vehicle, entering a first iteration stage, and when the course angle reaches pi / 4, entering a second iteration stage to calculate a subsequent pose point; and entering a third iteration stage to calculate pose points in the vehicle retreating process, and entering a fourth iteration stage to calculate all subsequent pose points when the course angle reaches pi / 4. The invention provides a solution of how to deal with the calculation error caused when the course angle is close to 90 degrees, and utilizes the obtained pose sequence to calculate the new pose, thereby greatly improving the calculation speed, reducing the calculation resources and improving the system response speed.

Description

Technical Field

[0001] The present invention belongs to the technical field of vehicle control, and relates to a method for calculating pose points of a large-arc circular curve path and a storage medium. Background Art

[0002] Automobile technology has developed very rapidly in recent years. Especially, a large number of technology companies, including Google, Huawei, Baidu, etc., have empowered automobiles with many new features, making automobiles no longer limited to a mechanical and electrical product, but a high-integration high-tech product integrating mechanical and electrical, control, computer, electronic information (chips), communication, materials, etc. "Electrification, intelligence, networking, and lightweight" have become the new four major development trends of automobiles. Currently, the level of intelligence has become the key publicity object of each host factory, and the technical route has developed from modularization a few years ago to end-to-end now. The rapid iteration of the technical architecture has brought about the rapid implementation of commercialization. Many passenger vehicle manufacturers have implemented NOA (Navigation Open Autopilot). Although the autonomous driving technology of commercial vehicles lags behind that of passenger vehicles, it is also rapidly catching up.

[0003] Motion planning and control (abbreviation of planning and control) is an important part of autonomous driving technology. Inside motion planning and control, discrete pose points are the output after planning, but the input of the control module. Therefore, discrete pose points play an important connecting role. Currently, the calculation methods for vehicle real-time pose points are basically based on vehicle kinematics calculation (which is the basic formula of vehicle theory), but how to apply and obtain actual discrete pose points is not clear. In actual application, it is found that directly using the discrete iteration formula of the kinematic equation will have some unexpected errors and the calculation efficiency is very low. Based on this, there is an urgent need to propose a method for calculating vehicle pose points on a curved path. Summary of the Invention

[0004] The purpose of the present invention is to propose a method for calculating pose points of a large-arc circular curve path to improve the efficiency of calculating the pose of existing discrete path points.

[0005] To achieve the above purpose, the present invention adopts the following technical solutions to solve the problem:

[0006] In a first aspect, the present invention provides a method for calculating pose points of a large-arc circular curve path, specifically including the following steps:

[0007] Step 1, continuously determine whether the vehicle is moving forward or backward. If the vehicle is moving forward, proceed to Step 2; if the vehicle is moving backward, proceed to Step 3;

[0008] Step 2, calculating the pose points during the forward movement of the vehicle:

[0009] During the vehicle's forward movement, given the initial pose (x0, y0, θ0) of the vehicle, the length Δs of a small segment of the arc change, the left front wheel turning angle δ, and the wheelbase L of the vehicle, it enters the first iteration stage. Specifically, the pose points are iteratively calculated using the following formula:

[0010] θ i+1 = θ i +(Δs * tanδ) / L

[0011] x i+1 = x i + Δs * cosθ i+1

[0012] y i+1 = y i + Δs * sinθ i+1 ;

[0013] In the formula:

[0014] θ i —The heading angle of the vehicle at the current moment;

[0015] θ i+1 —The heading angle of the vehicle at the next moment;

[0016] Δs—The length of a small segment of the arc change;

[0017] δ—The front wheel turning angle of the vehicle;

[0018] L—The wheelbase of the vehicle;

[0019] x i —The lateral displacement coordinate of the vehicle at the current moment;

[0020] x i+1 —The lateral displacement coordinate of the vehicle at the next moment;

[0021] y i —The longitudinal displacement coordinate of the vehicle at the current moment;

[0022] y i+1 —The longitudinal displacement coordinate of the vehicle at the next moment;

[0023] During the above calculation process, it is continuously judged whether θ i+1 is equal to π / 4. If so, the number of iterations in the first iteration stage is recorded as n, and it enters the second iteration stage. Specifically, the subsequent pose points are calculated using the following formula;

[0024] P n+m (x) = P n (x) - [P n-m (y) - P m (y)]

[0025] P n+m P(y) n P(y) - [P n-m (x) - P n (x) ]

[0026] Where:

[0027] P n+1 (x) — The horizontal coordinate value of the pose point P n+1 ;

[0028] P n+1 (y) — The vertical coordinate value of the pose point P n+1 ;

[0029] m — The current iteration number in the second stage, m = 1, 2, ……, n;

[0030] Step 3, Calculation of the pose point during vehicle reverse:

[0031] During vehicle reverse, given the starting pose (x0, y0, θ0) of the vehicle, the length of the small change segment of the arc Δs, the front wheel angle δ of the vehicle, and the wheelbase L of the vehicle, enter the third iteration stage, and calculate the pose point using the following formula:

[0032] θ i+1 = θ i + (Δs * tanδ) / L

[0033] x i+1 = x i - Δs * cosθ i+1

[0034] y i+1 = y i - Δs * sinθ i+1 ;

[0035] During the above calculation process, continuously judge whether θ i+1 is equal to π / 4. If so, record the iteration number of the third iteration stage as n, and enter the fourth iteration stage. Specifically, calculate all subsequent pose points using the following formula:

[0036] P n+m (x) = P n (x) - [P n-m (y) - P n (y) ]

[0037] P n+m (y) = P n (y) - [P n-m (x) - P n (x) ]

[0038] Where:

[0039] P n+1 (x) — The horizontal coordinate value of the pose point P n+1 ;

[0040] P m+1 (y) — The vertical coordinate value of the pose point P n+1 ;

[0041] m — The current iteration number in the fourth stage, m = 1, 2, ……, n.

[0042] In a second aspect, the present invention provides an electronic device, which includes:

[0043] A memory for storing executable instructions;

[0044] A processor, when executing the executable instructions or computer programs stored in the memory, implements the method for calculating the pose points of the large - arc circular curve path of the present invention as described above.

[0045] In a third aspect, the present invention provides a computer - readable storage medium storing executable instructions or computer programs, and when the executable instructions are executed by a processor, the method for calculating the pose points of the large - arc circular curve path of the present invention as described above is implemented.

[0046] Compared with the prior art, the present invention has the following technical effects:

[0047] (1) Discrete motion iterative formulas for vehicle forward and reverse are given. First, the heading angle should be calculated, and then the horizontal and vertical coordinates are calculated based on the updated heading angle. In automotive books, the heading - angle calculation formula is generally placed after the horizontal - and - vertical - coordinate calculation formulas, which would lead to calculating the horizontal and vertical coordinates first and then the heading angle. However, in practical applications, the update of the horizontal and vertical coordinates is related to the latest heading angle. The present invention gives the correct discrete pose iteration formula for application, rather than the theoretical derivation result on paper.

[0048] (2) The present invention proposes a solution to handle the calculation error caused when the heading angle is close to 90°, and uses the obtained pose sequence to calculate the new pose, greatly improving the calculation speed, reducing the calculation resources, and enhancing the system response speed. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] Figure 1 Shows the relationship between the tangent function and the independent variable;

[0050] Figure 2 The circular - arc path is in the second quadrant.

[0051] The present invention is further explained below in conjunction with the drawings and specific embodiments. SPECIFIC EMBODIMENTS

[0052] When performing autonomous driving path planning for a vehicle, regardless of the path planning algorithm used, the planned path usually consists of straight line segments, clothoid segments, and circular arc segments. Further, when performing path tracking control, the path is usually discretized to obtain discrete path points, and then control algorithms such as the pure pursuit algorithm are used to control the longitudinal and lateral movements of the vehicle to track the discrete path points.

[0053] For the circular arc segment path, the mathematical expression of the circular arc is the general formula of a circle, but problems will occur during the discretization process of the circular arc. For example: Assuming that when the vehicle is moving forward, the length of the tiny change segment of the circular arc is Δs, the following calculation formulas for the lateral and longitudinal coordinates of the vehicle displacement are usually used:

[0054]

[0055] In the formula:

[0056] θ i —The heading angle of the vehicle at the current moment;

[0057] θ i+1 —The heading angle of the vehicle at the next moment;

[0058] Δs—The length of the tiny change segment of the circular arc;

[0059] δ—The front wheel steering angle of the vehicle;

[0060] L—The wheelbase of the vehicle;

[0061] x i —The lateral coordinate of the vehicle displacement at the current moment;

[0062] x i+1 —The lateral coordinate of the vehicle displacement at the next moment;

[0063] y i —The longitudinal coordinate of the vehicle displacement at the current moment;

[0064] y i+1 —The longitudinal coordinate of the vehicle displacement at the next moment;

[0065] In the following three cases, problems will occur in the calculation results of the vehicle displacement lateral and longitudinal coordinates:

[0066] (1) When the heading angle θ of the vehicle at any moment is π / 2, At this time, the calculation result using the above formula obviously does not conform to the actual situation, so and (see Figure 1 );

[0067] (2) When the heading angle θ of the vehicle approaches π / 2 at any moment, the curve coordinate values become increasingly sensitive to the step size, and it may occur that the change in the lateral x value is much smaller than the change in the longitudinal y value, resulting in a result that does not conform to the actual situation;

[0068] (3) When it is limited that the heading angle θ of the vehicle does not exceed π / 2 at any moment, an incorrect situation where the calculated heading angle exceeds π / 2 may occur.

[0069] To solve the above problems, the present invention proposes a method for calculating the pose points of a large - arc circular curve path when the heading angle ranges from 45° to 90°.

[0070] Assume that the heading angle of the starting pose of the planned circular arc path is 0°, and the heading angle of the ending pose is 90° (i.e., π / 2), that is, the heading angle of the vehicle changes from 0° to 90°, and the planned path is approximately Figure 2 the circular arc in the second quadrant in, and this circular arc path is symmetric about the position point of the heading angle 45°.

[0071] Let the position at the heading angle 45° be P n (x, y), and the initial position be P0(x, y). Then (taking Figure 2 the second quadrant as an example for illustration):

[0072] P n+m (x) = P n (x) - [P n-m (y) - P n (y)]

[0073] P n+m (y) = P n (y) - [P n-m (x) - P n (x)] (2)

[0074] Where:

[0075] P n+1 (x) — the lateral coordinate value of point P n+1 ;

[0076] P n+1 (y) — the longitudinal coordinate of point P n+1 ;

[0077] m — the current iteration number in the second stage, m = 1, 2, ……, n;

[0078] According to the above algorithm (Formula 1), only the pose sequence with the heading angle ranging from 0° to 45° needs to be iteratively calculated based on vehicle kinematics. According to the pose point calculation method for the large-arc curve path when the heading angle ranges from 45° to 90° proposed in the present invention above, the pose point sequence with the heading angle ranging from 45° to 90° can be obtained.

[0079] Iteratively calculate the discrete pose points of the circular arc path according to the method proposed in the present invention.

[0080] As described above, the present invention provides a method for calculating pose points of a large-arc curve path, which specifically includes the following steps:

[0081] Step 1: Continuously determine whether the vehicle is moving forward or backward (this determination operation is a well-known means and does not need to be described). If the vehicle is moving forward, go to Step 2; if the vehicle is moving backward, go to Step 3.

[0082] Step 2: Calculation of pose points during the vehicle's forward movement:

[0083] During the vehicle's forward movement, given the starting pose (x0, y0, θ0) of the vehicle, the length Δs of the tiny change segment of the circular arc, the left front wheel turning angle δ of the vehicle, and the wheelbase L of the vehicle, enter the first iteration stage. Specifically, the following formula is used to iteratively calculate the pose points:

[0084] θ i+1 = θ i +(Δs * tanδ) / L

[0085] x i+1 = x i + Δs * cosθ i+1

[0086] y i+1 = y i + Δs * sinθ i+1 ;

[0087] In the formula:

[0088] θ i —The heading angle of the vehicle at the current moment;

[0089] θ i+1 —The heading angle of the vehicle at the next moment;

[0090] Δs — The length of the tiny change segment of the circular arc;

[0091] δ — The front wheel turning angle of the vehicle;

[0092] L — The wheelbase of the vehicle;

[0093] x i —The lateral displacement coordinate of the vehicle at the current moment;

[0094] x i+1 — The lateral displacement coordinate of the vehicle at the next moment;

[0095] y i — The longitudinal displacement coordinate of the vehicle at the current moment;

[0096] y i+1 — The longitudinal displacement coordinate of the vehicle at the next moment;

[0097] During the above calculation process, it is judged in real time whether θ i+1 is equal to π / 4. If so, record the number of iterations in the first iteration stage as n and enter the second iteration stage. Specifically, the following formula is used to calculate the subsequent pose points;

[0098] P n+m (x) = P n (x) - [P n-m (y) - P n (y)]

[0099] P n+m (y) = P n (y) - [P n-m (x) - P n (x)]

[0100] Where:

[0101] P n+1 (x) — The lateral coordinate value of the pose point P n+1 ;

[0102] P n+1 (y) — The longitudinal coordinate value of the pose point P n+1 ;

[0103] m — The current iteration number in the second stage, m = 1, 2, ……, n;

[0104] Step 3, Calculation of the pose points during the vehicle's reverse process:

[0105] During the vehicle's reverse process, given the starting pose (x0, y0, θ0) of the vehicle, the length Δs of the small change segment of the arc, the front wheel angle δ of the vehicle, and the wheelbase L of the vehicle, enter the third iteration stage and use Equation (3) to calculate the pose points. Equation (3) is derived from Equation (1):

[0106]

[0107] During the above calculation process, it is judged in real time whether θ i+1 is equal to π / 4. If so, record the number of iterations in the third iteration stage as n and enter the fourth iteration stage. Specifically, the following formula is used to calculate all subsequent pose points:

[0108] P n+m (x) = P n (x) - [P n-m (y) - P n (y)]

[0109] P n+m (y) = P n (y) - [P n-m (x) - P n (x)]

[0110] Where:

[0111] P n+1 (x) — The horizontal coordinate value of the pose point P n+1 ;

[0112] P n+1 (y) — The vertical coordinate value of the pose point P n+1 ;

[0113] m — The current iteration number in the fourth stage, m = 1, 2, ……, n;

[0114] To prove the feasibility and effectiveness of the present invention, specific embodiments are given below.

[0115] Embodiment 1:

[0116] During the forward movement, assuming the starting pose is (x0, y0, θ0) = (0, 0, 0), the length of the small change segment of the arc Δs = 1m, the left turn angle of the front wheel δ = 45°, L = 5m: Enter the first iteration stage:

[0117] Iteration 1:

[0118]

[0119] x1 = x0 + Δs * cosθ1 = 0 + cos 0.2 = 0.98m

[0120] y1 = y0 + Δs * sinθ1 = 0 + sin 0.2 = 0.199m;

[0121] Iteration 2:

[0122]

[0123] x2 = x1 + Δs * cosθ2 = 0.98 + cos 0.4 = 1.9m

[0124] y2 = y1 + Δs * sinθ2 = 0.199 + sin 0.4 = 0.59m;

[0125] Iteration 3:

[0126]

[0127] x3 = x2 + Δs * cosθ3 = 1.9 + cos 0.6 = 2.725 m

[0128] y3 = y2 + Δs * sinθ3 = 0.59 + sin 0.6 = 1.15 m;

[0129] Iteration 4:

[0130]

[0131] x4 = x3 + Δs * cosθ4 = 2.725 + cos 0.8 = 3.42 m

[0132] y4 = y3 + Δs * sinθ4 = 1.15 + sin 0.8 = 1.87 m;

[0133] Since π / 4 ≈ 0.8, at this time the number of iterations n in the first stage is 4, then the maximum value of m is 4, and it enters the second iteration stage. Specifically, the subsequent pose points are calculated using Equation (2):

[0134] Iteration 5:

[0135]

[0136] x5 = x4 - (y3 - y4) = 3.42 - (-0.717) = 4.137 m

[0137] y5 = y4 - (x3 - x4) = 1.87 - (-0.697) = 2.567 m;

[0138] Iteration 6:

[0139]

[0140] x6 = x4 - (y2 - y4) = 3.42 - (-1.28) = 4.7 m

[0141] y6 = y4 - (x2 - x4) = 1.87 - (-1.52) = 3.39 m;

[0142] Iteration 7:

[0143]

[0144] x7 = x4 - (y1 - y4) = 3.42 - (-1.67) = 5.09 m

[0145] y7 = y4 - (x1 - x4) = 1.87 - (-2.44) = 4.31 m;

[0146] The 8th iteration:

[0147]

[0148] x8 = x4 - (y0 - y4) = 3.42 + 1.87 = 5.29 m

[0149] y8 = y4 - (x0 - x4) = 1.87 - (-3.42) = 5.29 m;

[0150] At this time Moreover, x8 = y8.

[0151] From the above, the pose points of the vehicle at each moment during forward movement can be obtained, and thus each discrete pose point on the arc path can be obtained, that is, the pose sequence is obtained.

[0152] For the reverse situation, assuming the starting pose is (x0, y0, θ0) = (0, 0, 0), the length of the small change segment of the arc Δs = 1 m, the front wheel steering angle δ = 45°, L = 5 m, entering the third iteration stage, specifically, the vehicle pose points are calculated using the following formula:

[0153] θ i+1 = θ i + (Δs * tanδ) * / L

[0154] x i+1 = x i - Δs * cosθ i+1

[0155] y i+1 = y i - Δs * sinθ i+1

[0156] The 1st iteration:

[0157]

[0158] x1 = x0 - Δs * cosθ1 = 0 - cos 0.2 = -0.98 m

[0159] y1 = y0 - Δs * sinθ1 = 0 - sin 0.2 = -0.199 m;

[0160] The 2nd iteration:

[0161]

[0162] x2 = x1 - Δs * cosθ2 = -0.98 - cos 0.4 = -1.9 m

[0163] y2 = y1 - Δs * sinθ2 = -0.199 - sin 0.4 = -0.59 m;

[0164] Iteration 3:

[0165]

[0166] x3 = x2 - Δs * cosθ3 = -1.9 - cos 0.6 = -2.725 m

[0167] y3 = y2 - Δs * sinθ3 = -0.59 - sin 0.6 = -1.15 m;

[0168] Iteration 4:

[0169]

[0170] x4 = x3 - Δs * cosθ4 = -2.725 - cos 0.8 = -3.42 m

[0171] y4 = y3 - Δs * sinθ4 = -1.15 - sin 0.8 = -1.87 m;

[0172] Since π / 4 ≈ 0.8, at this time, the number of iterations n in the third stage is 4, so the maximum value of m is 4, entering the fourth iteration stage, calculating the subsequent pose points:

[0173] Iteration 5:

[0174]

[0175] x5 = x4 - (y3 - y4) = -3.42 - 0.717 = -4.137 m

[0176] y5 = y4 - (x3 - x4) = -1.87 - 0.697 = -2.567 m;

[0177] Iteration 6:

[0178]

[0179] x6 = x4 - (y2 - y4) = -3.42 - 1.28 = -4.7 m

[0180] y6 = y4 - (x2 - x4) = -1.87 - 1.52 = -3.39 m;

[0181] Iteration 7:

[0182]

[0183] x7 = x4 - (y1 - y4) = -3.42 - 1.67 = -5.09 m

[0184] y7 = y4 - (x1 - x4) = -1.87 - 2.44 = -4.31 m;

[0185] Iteration 8:

[0186]

[0187] x8 = x4 - (y0 - y4) = -3.42 - 1.87 = -5.29 m

[0188] y8 = y4 - (x0 - x4) = -1.87 - 3.42 = -5.29 m;

[0189] At this time Moreover, x8 = y8.

[0190] From the above, the pose points of the vehicle at each moment during the reverse process can be obtained, and thus each discrete pose point on the arc path can be obtained, that is, the pose sequence is obtained.

[0191] From the above example, it can be seen that a total of 8 iterations are performed during forward or reverse, which is equivalent to using 8 pose points to represent a 90° arc. Among them, the last 4 times are obtained based on the calculation results of the previous 4 times, and the trigonometric function calculations of the heading angle are not involved in the last 4 times. In this way, the calculation error caused by function error is avoided, and the calculation amount is greatly simplified. If more refined results are desired, the length Δs of the tiny change segment of the arc can be reduced. After reduction, the number of iterations will increase, and the specific value depends on the actual accuracy requirements.

Claims

1. A method for calculating pose points of a large-arc circular curve path, characterized in that, Specifically, it includes the following steps: Step 1: Continuously determine whether the vehicle is moving forward or backward. If the vehicle is moving forward, proceed to Step 2; if the vehicle is moving backward, proceed to Step 3. Step 2: Calculation of the pose points during the forward movement of the vehicle: During the forward movement of the vehicle, given the starting pose (x0, y0, θ0) of the vehicle, the length Δs of the tiny arc change segment, the left front wheel turning angle δ of the vehicle, and the wheelbase L of the vehicle, enter the first iteration stage. Specifically, use the following formula to iteratively calculate the pose points: θ i+1 = θ i + (Δs * tanδ) * / L x i+1 = x i + Δs * cosθ i+1 y i+1 = y i + Δs * sin θ i+1 ; In the formula: θ i — The heading angle of the vehicle at the current moment; θ i+1 — The heading angle of the vehicle at the next moment; Δs—the length of the tiny arc change segment; δ—the turning angle of the front wheels of the vehicle; L—the wheelbase of the vehicle; x i — Lateral coordinate of the vehicle's displacement at the current moment; x i+1 — Lateral coordinate of the vehicle's displacement at the next moment; y i — Longitudinal displacement coordinate of the vehicle at the current moment; y i+1 — Longitudinal coordinate of the vehicle's displacement at the next moment; During the above calculation process, it is determined in real time whether θ i+1 is equal to π / 4. If so, the number of iterations in the first iteration stage is recorded as n, and the second iteration stage is entered. Specifically, the following formula is used to calculate the subsequent pose points; P n+m P(x) = n P(x) - n-m P(y) - n P(y)]] P n+m (y) = P n (y) - [P n-m (x) - P n (x)] Where: P n+1 (x) — The horizontal coordinate value of the pose point P n+1 ; P n+1 (y) — Pose point P n+1 The longitudinal coordinate value of; m—the current iteration number in the second stage, m = 1, 2, ……, n; Step 3: Calculation of the pose points during the backward movement of the vehicle: During the backward movement of the vehicle, given the starting pose (x0, y0, θ0) of the vehicle, the length Δs of the tiny arc change segment, the turning angle δ of the front wheels of the vehicle, and the wheelbase L of the vehicle, enter the third iteration stage, and use the following formula to calculate the pose points: θ i+1 = θ i + (Δs * tanδ) * / L x i+1 = x i -Δs * cosθ i+1 y i+1 = y i -Δs * sinθ i+1 ; During the above calculation process, it is determined in real time whether θ i+1 is equal to π / 4. If so, the number of iterations in the third iteration stage is recorded as n, and the fourth iteration stage is entered. Specifically, the following formula is used to calculate all subsequent pose points: P n+m P(x) = n P(x) - n-m P(y) - n P(y)] P n+m (y) = P n (y) - [P n-m (x) - P n (x)] Where: P n+1 (x) — The lateral coordinate value of pose point P n+1 ; P m+1 (y) - Pose point P n+1 The longitudinal coordinate value of; m—the current iteration number in the fourth stage, m = 1, 2, ……, n.

2. An electronic device, characterized in that, The electronic device includes: A memory for storing executable instructions; A processor for implementing the method for calculating the pose points of the large - arc circular curve path as described in Claim 1 when executing the executable instructions or computer programs stored in the memory.

3. A computer-readable storage medium storing executable instructions or a computer program, characterized in that, The executable instructions, when executed by the processor, implement the method for calculating the pose points of the large - arc circular curve path as described in Claim 1.

Images