Pure tracking algorithm, device, vehicle and storage medium for determining preview point

Abstract

The application provides a pure tracking algorithm preview point determination method, device, vehicle and storage medium. The preview point determination method comprises the following steps: obtaining a current pose and a planning path of the vehicle; determining a current following point from a plurality of path points in the planning path according to a relative position relationship between the current pose of the vehicle and the plurality of path points; determining a first reference point from the path points arranged after the current following point in the planning path according to the shape of the planning path; and determining a preview point according to the position of the first reference point. The preview point determination method provided by the application can select a suitable preview distance according to the shape of the planning path, can avoid the problems of vehicle control oscillation caused by too short preview distance and low steering rationality caused by too long preview distance, and can improve the robustness of the vehicle following the path.

Description

Technical Field

[0001] This application relates to the field of automatic parking technology, and in particular to a method, apparatus, vehicle, and storage medium for determining the aiming point using a pure tracking algorithm. Background Technology

[0002] APA (Auto Parking Assist System) is a function that generates a planned path based on the target parking space, and then uses the planned path, vehicle status data, and vehicle positioning data to perform path tracking control (including longitudinal speed control and lateral steering control) to guide the vehicle to move to the target parking space. In APA mode, the robustness of the path tracking control method directly affects the safety and comfort of the occupants during the movement.

[0003] Currently, lateral control technology mainly adopts the Pure Pursuit algorithm. Existing Pure Pursuit algorithms generally determine the aiming point based on a fixed aiming distance. Research has found that existing Pure Pursuit algorithms have poor robustness: if the aiming distance is set too short, it will cause instability or even oscillation in vehicle control, resulting in insufficient control stability of the Pure Pursuit algorithm; if the aiming distance is set too long, the vehicle will exhibit an inside-cut phenomenon when following the planned turning path, that is, the vehicle understeers at large corners, resulting in low steering rationality of the Pure Pursuit algorithm. Summary of the Invention

[0004] In view of this, the main purpose of this application is to propose a method, apparatus, vehicle and storage medium for determining the aiming point of a pure tracking algorithm, in order to solve the problem of poor robustness of existing pure tracking algorithms.

[0005] To achieve the above objectives, the first aspect of this application provides a method for determining the aiming point of a pure tracking algorithm, applied to a vehicle, wherein the method for determining the aiming point obtains the current pose and planned path of the vehicle; wherein the planned path includes multiple path points;

[0006] Based on the relative positional relationship between the vehicle's current pose and multiple path points in the planned path, the current following point is determined from the multiple path points in the planned path;

[0007] Based on the shape of the planned path, a first reference point is determined from the path points arranged after the current following point in the planned path; and

[0008] The aiming point is determined based on the position of the first reference point;

[0009] Wherein, the path point refers to a position point in the planned path; the current following point is the path point among multiple path points in the planned path that is located ahead of the vehicle's current direction of movement and is closest to the vehicle; the first reference point is a path point that extends a certain distance along the planned path towards the vehicle's current direction of movement based on the current following point, and the extension distance is related to the shape of the planned path; the aiming point is the current target point determined based on the first reference point, used to calculate the target steering wheel angle of the vehicle.

[0010] The aiming point determination method provided in this application determines the current following point based on the relative positional relationship between the vehicle's current pose and multiple path points in the planned path. Then, based on the shape of the planned path, a first reference point is obtained by extending a corresponding distance forward from the current following point. Finally, the aiming point is determined based on the position of the first reference point. In this way, an appropriate extension distance can be selected to obtain the aiming point according to the shape of the planned path. This avoids the problem of vehicle control oscillation caused by an excessively short aiming distance and the problem of low steering rationality caused by an excessively long aiming distance, thereby improving the robustness of the vehicle following the path.

[0011] In some embodiments, determining the current following point from multiple path points in the planned path based on the relative positional relationship between the vehicle's current pose and multiple path points in the planned path includes:

[0012] A coordinate system is established with the preset part of the vehicle as the origin, and the position coordinates of each path point in the planned path are determined in the coordinate system.

[0013] Determine whether the vehicle is currently moving forward or backward;

[0014] If the vehicle's current direction of movement is forward, then the path point with the smallest absolute value of the ordinate that is greater than zero among the multiple path points in the planned path is determined as the current following point; wherein, the direction of the extension of the ordinate is the direction from the preset part of the vehicle to the front of the vehicle.

[0015] If the current direction of the vehicle's movement is backward, then the path point with the smallest absolute value of the ordinate that is less than zero among the multiple path points in the planned path is determined as the current following point.

[0016] The first following point is determined based on the position of the current following point in the planned path;

[0017] Determine whether the current following point is the first path point in the planned path;

[0018] If the current following point is the first path point in the planned path, then the first path point in the planned path following the current following point is determined as the first following point; and

[0019] If the current following point is not the first path point in the planned path, then the current following point is determined as the first following point.

[0020] Thus, depending on whether the current following point is the first following point in the planned path, there are two cases to select the first following point, which can improve the accuracy of the pre-aiming point.

[0021] In some embodiments, the shape of the planned path includes a straight line, a single-sided turning arc, and an S-shape, wherein the single-sided turning arc includes a left-turning arc and a right-turning arc;

[0022] Determining a first reference point from the path points arranged after the current following point in the planned path, based on the shape of the planned path, includes:

[0023] Determine whether the planned path is a straight line, a single-sided curved arc, or an S-shape;

[0024] If the planned path is a straight line, then based on the relationship between the number of path points arranged after the first following point and L, the Lth path point arranged after the first following point in the planned path is determined as the first reference point, or the last path point in the planned path is determined as the first reference point.

[0025] If the planned path is a single-sided curved arc, then based on the relationship between the number of path points following the first following point and M, the Mth path point following the first following point is determined as the first reference point; or, the last path point in the planned path is determined as the first reference point.

[0026] If the planned path is S-shaped, then based on the relationship between the number of path points arranged after the first following point and N, the Nth path point arranged after the first following point in the planned path is determined as the first reference point, or the last path point in the planned path is determined as the first reference point; where L is the first preset number of forward path points, M is the second preset number of forward path points, N is the third preset number of forward path points, and L>M>0, L>N>0.

[0027] Thus, by setting L>M and L>N, when the vehicle is moving along a straight planned path, the aiming point is set further away, which can avoid the problem of vehicle control oscillation caused by the aiming distance being too short; when the vehicle is moving along a single-sided turning arc or S-shaped planned path, the aiming point is set closer, which can avoid the problem of low steering rationality caused by the aiming distance being too long.

[0028] In some embodiments, determining the Lth path point in the planned path after the first following point as the first reference point based on the relationship between the number of path points in the planned path following the first following point and L, or determining the last path point in the planned path as the first reference point, includes:

[0029] Determine whether the number of path points arranged after the first following point in the planned path is greater than L;

[0030] If the number of path points following the first follower point in the planned path is greater than L, then the Lth path point following the first follower point in the planned path is determined as the first reference point; and

[0031] If the number of path points arranged after the first following point in the planned path is less than or equal to L, then the last path point in the planned path is determined as the first reference point.

[0032] The step of determining the Mth path point in the planned path after the first following point as the first reference point, based on the relationship between the number of path points in the planned path following the first following point and M, or determining the last path point in the planned path as the first reference point, includes:

[0033] Determine whether the number of path points arranged after the first following point in the planned path is greater than M;

[0034] If the number of path points following the first follower point in the planned path is greater than M, then the Mth path point following the first follower point in the planned path is determined as the first reference point; and

[0035] If the number of path points arranged after the first following point in the planned path is less than or equal to M, then the last path point in the planned path is determined as the first reference point.

[0036] The step of determining the Nth path point in the planned path after the first following point as the first reference point, based on the relationship between the number of path points in the planned path following the first following point and N, or determining the last path point in the planned path as the first reference point, includes:

[0037] Determine whether the number of path points arranged after the first following point in the planned path is greater than N;

[0038] If the number of path points following the first follower point in the planned path is greater than N, then the Nth path point following the first follower point in the planned path is determined as the first reference point; and

[0039] If the number of path points arranged after the first following point in the planned path is less than or equal to N, then the last path point in the planned path is determined as the first reference point.

[0040] Thus, when the vehicle moves to near the end of the planned path, the last path point is determined as the first reference point, which can avoid the situation where the aiming point cannot be determined.

[0041] In some embodiments, determining the first following point based on the position of the current following point in the planned path further includes:

[0042] After determining the first following point, the first path point in the planned path that precedes the current following point is determined as the second following point; and

[0043] Calculate the first projection distance of the vector from the second following point to the current pose of the vehicle onto the vector from the second following point to the first following point;

[0044] Determining the aiming point based on the position of the first reference point includes:

[0045] Determine whether the first reference point is the last path point in the planned path;

[0046] If the first reference point is the last path point in the planned path, then determine whether the shape of the planned path is a straight line;

[0047] If the planned path is a straight line, then the position point extending forward a first preset distance along the orientation of the first reference point is determined as the aiming point;

[0048] If the planned path is not a straight line, then based on the total number of path points in the planned path, a point extending forward a second preset distance along the orientation of the first reference point is determined as the aiming point; or, a point extending forward a third preset distance along the turning arc where the first reference point is located is determined as the aiming point, wherein the first preset distance > the second preset distance ≥ the third preset distance; and

[0049] If the first reference point is not the last path point in the planned path, then the pre-aiming point is determined based on the first projection distance.

[0050] Thus, when the vehicle moves to near the end of the planned path, the aiming point is obtained by extending forward from the last path point of the planned path, thereby solving the problem of losing the aiming point when the vehicle moves to the last path point.

[0051] In some embodiments, determining a first reference point from path points arranged after the current following point in the planned path, based on the shape of the planned path, further includes:

[0052] After determining the first reference point, the first path point in the planned path that follows the first reference point is determined as the second reference point;

[0053] Determining the aiming point based on the first projection distance includes:

[0054] Determine whether the first projection distance is greater than 0;

[0055] If the first projection distance is less than or equal to 0, then the first reference point is determined as the pre-aiming point; and

[0056] If the first projection distance is greater than 0, then the pre-aiming point is obtained by extending along the first reference point to the second reference point based on the curvature of the first reference point, the curvature of the second reference point, and the first projection distance.

[0057] Therefore, when the first reference point is not the last path point in the planned path, the pre-aiming point is determined based on the positional relationship between the vehicle's pose and the first following point, as well as the position of the first reference point. This allows the pre-aiming point to move with the movement of the vehicle, thereby solving the problems of non-linear, uneven, and jerky steering wheel control during the vehicle's path following process.

[0058] In some embodiments, the step of extending the aiming point from the first reference point to the second reference point based on the curvature of the first reference point, the curvature of the second reference point, and the first projection distance includes:

[0059] Determine whether the curvature of the first reference point and the curvature of the second reference point satisfy any one of three preset conditions;

[0060] If the curvature of the first reference point and the curvature of the second reference point satisfy any one of the three preset conditions, then the position point extending forward by a first projection distance from the first reference point along the direction pointing from the first reference point to the second reference point is determined as the aiming point;

[0061] If the curvature of the first reference point and the curvature of the second reference point do not satisfy any of the three preset conditions, then the aiming point search step is repeated along the arc segment from the first reference point to the second reference point, with the first reference point as the initial parent node, to obtain the aiming point. Here, the parent node refers to the starting point for executing the aiming point search step.

[0062] The pre-aiming point search step includes:

[0063] Starting from the parent node, extend along the arc segment from the first reference point to the second reference point according to a preset step size to obtain candidate position points;

[0064] Determine whether the second projection distance of the vector from the first reference point to the candidate position point on the vector from the first reference point to the second reference point is greater than or equal to the first projection distance;

[0065] If the second projection distance of the vector from the first reference point to the candidate location point on the vector from the first reference point to the second reference point is less than the first projection distance, then the pre-aiming point search step is continued with the candidate location point as the new parent node; and

[0066] If the second projection distance of the vector from the first reference point to the candidate position point on the vector from the first reference point to the second reference point is greater than or equal to the first projection distance, then the candidate position point is determined as the pre-aiming point, and the pre-aiming point search step ends.

[0067] The three preset conditions include:

[0068] The absolute value of the curvature of the first reference point is less than a preset curvature threshold, and the absolute value of the difference between the curvature of the first reference point and the curvature of the second reference point is less than a first preset curvature difference.

[0069] The absolute value of the difference between the curvature of the first reference point and the curvature of the second reference point is greater than the second preset curvature difference; and

[0070] The curvature of the first reference point has the opposite sign to the curvature of the second reference point.

[0071] This reduces the amount of computation and avoids program errors.

[0072] In some embodiments, the value of M includes M1, M2, and M3; wherein, M1>M2>M3;

[0073] The step of determining the Mth path point in the planned path that follows the first following point as the first reference point includes:

[0074] Determine the relationship between the total number of path points in the planned path and a first preset threshold and a second preset threshold; wherein the first preset threshold is greater than the second preset threshold;

[0075] If the total number of path points in the planned path is greater than the first preset number threshold, then the M1th path point in the planned path that follows the first following point is determined as the first reference point.

[0076] If the total number of path points in the planned path is less than or equal to the first preset number threshold and greater than the second preset number threshold, then the M2th path point in the planned path after the first following point is determined as the first reference point.

[0077] If the total number of path points in the planned path is less than or equal to the second preset threshold, then the M3th path point in the planned path that follows the first following point is determined as the first reference point.

[0078] Thus, by setting M1>M2>M3, when the vehicle follows the planned path of a single-sided turning arc, the aiming distance is selected based on the total number of path points X, that is, based on the total length of the planned path. When the total length of the planned path is short, a shorter aiming distance is selected accordingly, which allows the vehicle to follow the planned path more closely. This further avoids the problem of low steering rationality caused by an excessively long aiming distance.

[0079] A second aspect of this application also provides a pre-aiming point determination device for a pure tracking algorithm, the device comprising a memory and a processor, the memory being connected to the processor, the memory storing a computer program, the computer program being executed by the processor to implement the pre-aiming point determination method for the pure tracking algorithm as described in the first aspect above.

[0080] A third aspect of this application also provides a vehicle, said vehicle comprising:

[0081] Vehicle body; and

[0082] As described in the second aspect above, the aiming point determination device of the pure tracking algorithm is mounted on the vehicle body. The aiming point determination device of the pure tracking algorithm is used to determine the aiming point for the vehicle, thereby controlling the movement of the vehicle based on the aiming point.

[0083] A fourth aspect of this application also provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements a pre-aiming point determination method for the pure tracking algorithm described in the first aspect above.

[0084] The aiming point determination device, vehicle, and storage medium for the pure tracking algorithm provided in this application can also implement the aiming point determination method for the pure tracking algorithm described in the first aspect above. The current following point is determined based on the relative positional relationship between the current pose of the vehicle and multiple path points in the planned path. Based on the shape of the planned path, a first reference point is obtained by extending a corresponding distance forward from the current following point. The aiming point is then determined based on the position of the first reference point. In this way, it is possible to select an appropriate extension distance to obtain the aiming point based on the shape of the planned path. This can avoid the problem of vehicle control oscillation caused by too short an aiming distance and the problem of low steering rationality caused by too long an aiming distance, thereby improving the robustness of the vehicle following the path.

[0085] Additional aspects and advantages of this application will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of this application. Attached Figure Description

[0086] Figure 1 A schematic diagram illustrating the calculation of the target steering wheel angle of a vehicle provided in an embodiment of this application;

[0087] Figure 2 A flowchart illustrating the steps of a pre-aiming point determination method for a pure tracking algorithm provided in this application embodiment;

[0088] Figure 3 for Figure 2 Detailed flowchart of step 20;

[0089] Figure 4 for Figure 3 The first detailed flowchart for step 25;

[0090] Figure 5a A schematic diagram illustrating the rules for determining the aiming point in the first scenario provided in this application embodiment;

[0091] Figure 5b This is a schematic diagram illustrating the rules for determining the aiming point in the second scenario provided in this application embodiment;

[0092] Figure 5c A schematic diagram illustrating the rules for determining the aiming point in the third scenario provided in this application embodiment;

[0093] Figure 5d A schematic diagram illustrating the rules for determining the aiming point in the fourth scenario provided in this application embodiment;

[0094] Figure 5e A schematic diagram illustrating the rules for determining the aiming point in the fifth scenario provided in this application embodiment;

[0095] Figure 6 for Figure 2 The first detailed flowchart of step 31;

[0096] Figure 7 for Figure 2 The second detailed flowchart for step 31;

[0097] Figure 8 for Figure 7 Detailed flowchart of step 3132;

[0098] Figure 9 for Figure 7 Detailed flowchart of step 3142;

[0099] Figure 10 for Figure 2 The first detailed flowchart for step 32;

[0100] Figure 11 for Figure 3 The second detailed flowchart for step 25;

[0101] Figure 12 for Figure 2 The second detailed flowchart for step 32;

[0102] Figure 13 for Figure 2 The third detailed flowchart of step 31;

[0103] Figure 14 for Figure 12 The second detailed flowchart for step 326;

[0104] Figure 15 for Figure 14 The second detailed flowchart of step 3263;

[0105] Figure 16 A flowchart of the pre-aiming point search steps provided in the embodiments of this application;

[0106] Figure 17This is a schematic diagram of a scenario for the pre-aiming point search step provided in an embodiment of this application;

[0107] Figure 18 A schematic diagram of a pre-aiming point determination device for a pure tracking algorithm provided in an embodiment of this application;

[0108] Figure 19 This is a schematic diagram of a vehicle provided in an embodiment of this application.

[0109] The annotations in the attached figures are explained as follows:

[0110] Vehicle 1

[0111] Pre-aiming point determination device 60

[0112] Memory 61

[0113] Processor 62

[0114] Computer Program 610

[0115] 80 vehicle body

[0116] Candidate location points D2 D1~D3

[0117] Sub-circular arc segments Y1~Y4

[0118] The following detailed description, in conjunction with the accompanying drawings, will further illustrate this application. Detailed Implementation

[0119] The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. All other embodiments obtained by those skilled in the art based on the embodiments of this application without creative effort are within the scope of protection of this application.

[0120] In the description of this application, it should be noted that the terms "first," "second," etc., are used for descriptive purposes only and should not be construed as indicating or implying relative importance.

[0121] During the operation of the APA (Auto Parking Assist System), the vehicle needs to follow the planned path. Specifically, the Path Follow (PF) module controls the vehicle's longitudinal speed and lateral steering based on the path information, vehicle status data, and vehicle positioning data provided by the Path Plan (PP) module, ensuring the vehicle follows the planned path generated by the PP module. Specifically, the PF module outputs a target steering wheel angle to the EPS (Electric Power Steering) system, which then controls the steering wheel to follow the target steering wheel angle, thereby controlling the vehicle's lateral steering.

[0122] The existing PF module's lateral control mainly uses a pure tracking algorithm. This algorithm requires calculating the curvature of the arc connecting the vehicle's rear axle center to the vehicle's aiming point. The position of the aiming point is determined by the aiming distance l. d Confirmed. Figure 1 As shown, the vehicle's steering angle δ can be determined using only the angle α between the vehicle's heading direction and the aiming point direction. Applying the sine law... Figure 1 The following formula can be obtained:

[0123]

[0124] Expressing the above formula in curvature form, we get:

[0125]

[0126] Where κ represents the curvature of the rear axle trajectory. On the other hand, based on the vehicle wheelbase H and the calculation formula for the feedforward angle of the PID (Proportion-Integral-Derivative) control, we can obtain:

[0127] δ=arctan(kH) (3)

[0128] Substituting the expression for κ, we obtain the control equations for the pure tracking algorithm as follows:

[0129]

[0130] According to lateral deviation From the definition, we can obtain:

[0131]

[0132] The curvature formula can then be rewritten as:

[0133]

[0134] Therefore, the control equations of the pure tracking algorithm can be rewritten as:

[0135]

[0136] According to the control equation (7) of the pure tracking algorithm, it can be seen that the steering angle δ of the vehicle obtained by the pure tracking algorithm is related to the lateral deviation. And the aiming distance l d Related. The aiming distance l of existing pure tracking algorithms d It is a fixed value, which leads to poor robustness of the vehicle following the path: if the set aiming distance l d If the aiming distance is too short, it will cause instability or even oscillation in vehicle control, resulting in insufficient control stability of the pure tracking algorithm; if the aiming distance is set too short, it will cause instability or even oscillation in vehicle control. d If the length is too long, the vehicle will exhibit an inside-cut phenomenon when following the planned turning path, meaning that the vehicle will understeer at large corners, resulting in low steering rationality of the pure tracking algorithm.

[0137] In view of this, please refer to Figure 2 This application provides a method for determining the aiming point of a pure tracking algorithm, which includes the following steps:

[0138] Step 10: Obtain the vehicle's current pose and planned path. The planned path includes multiple waypoints.

[0139] Step 20: Determine the current following point from among the multiple path points in the planned path based on the relative positional relationship between the vehicle's current pose and the multiple path points in the planned path.

[0140] Step 31: Based on the shape of the planned path, determine the first reference point from the path points arranged after the current following point in the planned path.

[0141] Step 32: Determine the aiming point based on the position of the first reference point.

[0142] For example, the planned path may be generated by the path planning module in the vehicle, and the distance between any two adjacent path points in the planned path is equal, for example, 0.3m.

[0143] It should be noted that, in this application, the path point refers to a virtually set position point (through point) when the vehicle performs path planning. The current following point is the path point among multiple path points in the planned path that is closest to the vehicle in its current direction of movement, and is used to indicate the approximate position of the vehicle's current posture in the planned path. The first reference point is a path point extending a certain distance along the planned path in the vehicle's current direction of movement based on the current following point. It is a preliminary aiming point used to provide a reference for accurately determining the aiming point. The extension distance is related to the shape of the planned path. The aiming point is the current target point determined based on the first reference point, used to calculate the target steering wheel angle of the vehicle, thereby controlling the vehicle to follow the planned path.

[0144] The aiming point determination method provided in this application determines the current following point based on the relative positional relationship between the vehicle's current pose and multiple path points in the planned path. Then, based on the shape of the planned path, a first reference point is obtained by extending a corresponding distance forward from the current following point. Finally, the aiming point is determined based on the position of the first reference point. In this way, an appropriate extension distance can be selected to obtain the aiming point according to the shape of the planned path. This avoids the problem of vehicle control oscillation caused by an excessively short aiming distance and the problem of low steering rationality caused by an excessively long aiming distance, thereby improving the robustness of the vehicle following the path.

[0145] It should be noted that, in this embodiment, the term "pose" includes the position of the vehicle and the angle at which the vehicle's front is facing.

[0146] Further, please refer to Figure 3 Step 20 includes steps 21 to 25, as follows:

[0147] Step 21: Establish a coordinate system with the preset part of the vehicle as the origin, and determine the position coordinates of each path point in the planned path in the coordinate system.

[0148] Step 22: Determine whether the vehicle is currently moving forward or backward. If the vehicle is currently moving forward, proceed to step 23. If the vehicle is currently moving backward, proceed to step 24.

[0149] Step 23: Determine the path point with the smallest absolute value of the ordinate that is greater than zero among the multiple path points in the planned path as the current following point.

[0150] Step 24: Determine the path point with the smallest absolute value of its ordinate that is less than zero among the multiple path points in the planned path as the current following point.

[0151] Step 25: Determine the first following point based on the position of the current following point in the planned path.

[0152] In this embodiment, the preset location of the vehicle is the rear axle center, and the coordinate system is a Cartesian coordinate system. The vertical axis of the Cartesian coordinate system extends from the rear axle center towards the front of the vehicle, and the horizontal axis extends perpendicularly to the vertical axis. The positional relationship of each path point relative to the vehicle refers to its positional relationship relative to the rear axle center. For example, if a path point is located in front of the vehicle's current direction of movement, it means that the direction from the rear axle center towards that path point is approximately the same as the vehicle's current direction of movement, i.e., the angle between the two directions is less than 90 degrees. Of course, in other embodiments, a coordinate system can be established using other points (e.g., the first path point in the planned path) to determine the vehicle's current following point; this is not limited here.

[0153] Understandably, when the vehicle is moving forward, the pathpoint with a ordinate greater than zero and the smallest absolute value is the pathpoint in front of the vehicle and closest to it; when the vehicle is moving backward, the pathpoint with a ordinate less than zero and the smallest absolute value is the pathpoint behind the vehicle and closest to it. Thus, determining the current following point based on the vehicle's direction of movement improves the accuracy of the pre-aiming point.

[0154] In one embodiment, please refer to Figure 4 Step 25 includes steps 251 to 253, as follows:

[0155] Step 251: Determine whether the current following point is the first path point in the planned path. If the current following point is the first path point in the planned path, proceed to step 252. If the current following point is not the first path point in the planned path, proceed to step 253.

[0156] Step 252: Determine the first path point in the planned path that follows the current following point as the first following point.

[0157] Step 253: Determine the current following point as the first following point.

[0158] For example, in Figure 5a In the scenario shown, the planned path includes 12 sequentially numbered path points. The vehicle's direction of movement is forward. The first path point is the vehicle's current following point, and the second path point is the first following point. Figure 5bIn the scenario shown, the planned path includes 16 sequentially numbered path points. The vehicle's direction of movement is forward. The ordinate of the first path point is exactly 0, and the ordinate of the second path point is greater than zero and has the smallest absolute value. Therefore, the second path point is the vehicle's current following point, also known as the first following point. In this application, when the current following point is the first path point in the planned path, the first path point in the planned path after the current following point, i.e., the second path point, is determined as the first following point. When the current following point is the first path point in the planned path, the current following point is determined as the first following point, and then a distance is extended forward from the first following point along the vehicle's current direction of movement to obtain a first reference point. It should be noted that the path points obtained during path planning are a series of virtual position points obtained by extending forward in the vehicle's direction of movement based on the vehicle's initial pose, speed, and other vehicle information. Therefore, the first path point in the planned path is located in front of the vehicle's initial pose.

[0159] In this embodiment, the shape of the planned path includes a straight line, a single-sided turning arc, and an S-shape, wherein the single-sided turning arc includes a left-turning arc and a right-turning arc. For example, the shape of the planned path can be determined based on the curvature corresponding to each path point. For instance, when the curvature of each path point is close to zero (i.e., the absolute value of the curvature is less than or equal to a preset curvature threshold κ0), the shape of the planned path can be determined to be a straight line (e.g., ...). Figure 5a As shown in the figure, when the curvature of several path points is greater than κ0, the shape of the planned path can be determined to be a right-turn arc (e.g., ...). Figure 5b As shown in the figure, when the curvature of some path points is greater than κ0, and the curvature of other path points is less than -κ0, the shape of the planned path can be determined to be S-shaped (e.g., ...). Figure 5c As shown in the figure, where κ0>0, for example, κ0=0.02. The curvature of a path point is the curvature of the rear axle trajectory when the vehicle moves to that path point.

[0160] Further, in this embodiment, when performing step 31, if the planned path is a straight line, a first extension distance (i.e., the distance corresponding to L path points extending forward from the first following point along the current direction of vehicle movement) is extended from the first following point to obtain a first reference point; if the planned path is a single-sided curved arc, a second extension distance (i.e., the distance corresponding to M path points extending forward from the first following point) is extended from the first following point along the current direction of vehicle movement to obtain a first reference point; if the planned path is S-shaped, a third extension distance (i.e., the distance corresponding to N path points extending forward from the first following point) is extended from the first following point along the current direction of vehicle movement to obtain a first reference point. Wherein, L is the first preset number of forward path points, M is the second preset number of forward path points, and N is the third preset number of forward path points. The first extension distance is greater than the second extension distance, and the first extension distance is greater than the third extension distance. For example, if L is 8 and the distance between two adjacent path points is 0.3m, then the first extension distance is 2.4m.

[0161] Specifically, please refer to Figure 6 Step 31 includes steps 311 to 314:

[0162] Step 311: Determine whether the planned path is a straight line, a single-sided curved path, or an S-shape. If the planned path is a straight line, proceed to step 312. If the planned path is a single-sided curved path, proceed to step 313. If the planned path is an S-shape, proceed to step 314.

[0163] Step 312: Based on the relationship between the number of path points arranged after the first following point in the planned path and L, determine the Lth path point arranged after the first following point in the planned path as the first reference point, or determine the last path point in the planned path as the first reference point.

[0164] Step 313: Based on the relationship between the number of path points arranged after the first following point in the planned path and M, determine the Mth path point arranged after the first following point in the planned path as the first reference point, or determine the last path point in the planned path as the first reference point.

[0165] Step 314: Based on the relationship between the number of path points arranged after the first following point in the planned path and N, determine the Nth path point arranged after the first following point in the planned path as the first reference point, or determine the last path point in the planned path as the first reference point.

[0166] Where L>M>0, L>N>0.

[0167] Thus, by setting L>M and L>N, when the vehicle is moving along a straight planned path, it moves forward more path points from the first following point to obtain the first reference point, thereby setting the aiming point further away and avoiding the problem of vehicle control oscillation caused by an excessively short aiming distance. When the vehicle is moving along a single-sided turning arc or S-shaped planned path, it moves forward more path points from the first following point to obtain the first reference point, thereby setting the aiming point closer and avoiding the problem of low steering rationality caused by an excessively long aiming distance.

[0168] Further, please refer to Figure 7 Step 312 includes steps 3121 to 3123, step 313 includes steps 3131 to 3133, and step 314 includes steps 3141 to 3143, as detailed below:

[0169] Step 3121: Determine whether the number of path points in the planned path following the first follower point is greater than L. If the number of path points in the planned path following the first follower point is greater than L, proceed to step 3122. If the number of path points in the planned path following the first follower point is less than or equal to L, proceed to step 3123.

[0170] Step 3122: Determine the Lth path point in the planned path that follows the first following point as the first reference point.

[0171] Step 3123: Determine the last path point in the planned path as the first reference point.

[0172] Step 3131: Determine whether the number of path points in the planned path following the first follower point is greater than M. If the number of path points in the planned path following the first follower point is greater than M, proceed to step 3132. If the number of path points in the planned path following the first follower point is less than or equal to M, proceed to step 3123.

[0173] Step 3132: Determine the Mth path point in the planned path that follows the first following point as the first reference point.

[0174] Step 3141: Determine whether the number of path points in the planned path following the first follower point is greater than N. If the number of path points in the planned path following the first follower point is greater than N, proceed to step 3142. If the number of path points in the planned path following the first follower point is less than or equal to N, proceed to step 3123.

[0175] Step 3142: Determine the Nth path point in the planned path that follows the first following point as the first reference point.

[0176] For example, in Figures 5c-5d In the scenario shown, assuming N=2, the planned path is S-shaped and includes 12 sequentially numbered path points, and the vehicle's direction of movement is forward. Figure 5c As shown, at this point, a coordinate system is established with the rear axle center of the vehicle as the origin. Among the multiple path points in the planned path, the 6th path point has a ordinate greater than zero and the absolute value of its ordinate is the smallest. Therefore, the 6th path point is the current following point of the vehicle, which is also the first following point. At this time, the number of path points after the first following point is 6, which is greater than N. Therefore, the 2nd path point after the first following point, i.e., the 8th path point, is the first reference point.

[0177] like Figure 5d As shown, at this point, a coordinate system is established with the rear axle center of the vehicle as the origin. Among the multiple path points in the planned path, the 11th path point has a ordinate greater than zero and the absolute value of its ordinate is the smallest. Therefore, the 11th path point is the current following point of the vehicle, which is also the first following point. At this time, the number of path points after the first following point is 1, which is less than N. Therefore, the last path point in the planned path, i.e., the 12th path point, is the first reference point.

[0178] Thus, when the vehicle moves to near the end of the planned path, the last path point is determined as the first reference point, which can avoid the situation where the aiming point cannot be determined.

[0179] It should be noted that the example of an S-shaped planned path used here to illustrate the step of determining the first reference point is not intended to limit this application. Similarly, in other steps of the embodiments of this application, when the planned path has other shapes, the examples of determining the first reference point are similar, and will not be elaborated here.

[0180] Furthermore, in this embodiment of the application, when performing step 3132, the value of M is specifically determined based on the relationship between the total number of path points X in the planned path and the first preset threshold X1 and the second preset threshold X2. The value of M includes M1, M2, and M3, where M1 > M2 > M3. Thus, when the vehicle follows a planned path with a single-sided turning arc, the aiming distance is selected based on the total number of path points X, i.e., the total length of the planned path. When the total length of the planned path is short, a shorter aiming distance is selected accordingly, allowing the vehicle to follow the planned path more closely. This further avoids the problem of low steering rationality caused by an excessively long aiming distance.

[0181] Specifically, please refer to Figure 8 Step 3132 includes steps 31321 to 31324:

[0182] Step 31321: Determine the relationship between the total number of path points X in the planned path and the first preset threshold X1 and the second preset threshold X2. If the total number of path points X in the planned path is greater than the first preset threshold X1, proceed to step 31322. If the total number of path points X in the planned path is less than or equal to the first preset threshold X1 and greater than the second preset threshold X2, proceed to step 31323. If the total number of path points X in the planned path is less than or equal to the second preset threshold X2, proceed to step 31324.

[0183] Wherein, the first preset number threshold X1 is greater than the second preset number threshold X2.

[0184] Step 31322: Determine the M1th path point in the planned path that follows the first following point as the first reference point.

[0185] Step 31323: Determine the M2th path point in the planned path that follows the first following point as the first reference point.

[0186] Step 31324: Determine the M3th path point in the planned path that follows the first following point as the first reference point.

[0187] For example, X1 = 15, X2 = 6, M1 = 6, M2 = 3, M3 = 1.

[0188] It should be noted that the values of X1, X2, M1, M2, and M3 can be flexibly selected according to the performance of the vehicle and the distance between two adjacent path points in the planned path, and can be obtained through experiments. The specific values exemplified in the embodiments of the present application are only for illustration to facilitate understanding of the embodiments provided by the present application, and do not represent that only the values exemplified in the present application can be selected, and should not be regarded as a limitation to the present application.

[0189] Further, in the embodiments of the present application, when performing step 3142, the value of N is specifically determined according to the running mode of the vehicle. The value of N includes N1 and N2. N1 < N2. Thus, when the running mode of the vehicle is the automatic parking mode, N takes the smaller value N1, which can make the vehicle more closely follow the planned route, thereby improving the safety of automatic parking. When the running mode of the vehicle is not the automatic parking mode, N takes the larger value N2, which can reduce the amount of calculation.

[0190] Specifically, please refer to Figure 9 and step 3142 includes steps 31421 to 31423:

[0191] Step 31421, determine whether the current running mode of the vehicle is the automatic parking mode. If the current running mode of the vehicle is the automatic parking mode, then execute step 31422. If the current running mode of the vehicle is not the automatic parking mode, then execute step 31423.

[0192] Step 31422, determine the N1th path point after the first following point in the planned path as the preview point.

[0193] Step 31423, determine the N2th path point after the first following point in the planned path as the preview point.

[0194] Exemplarily, N1 = 4 and N2 = 2.

[0195] It should be noted that the values of N1 and N2 can be flexibly selected according to the performance of the vehicle and the distance between two adjacent path points in the planned path, and can be obtained through experiments. The specific values exemplified in the embodiments of the present application are only for illustration to facilitate understanding of the embodiments provided by the present application, and do not represent that only the values exemplified in the present application can be selected, and should not be regarded as a limitation to the present application.

[0196] Further, in one embodiment, step 32 includes: determining the first reference point as the preview point. In this embodiment, the first reference point is directly determined as the preview point, and the control is simple.

[0197] It has been found through research that when the vehicle moves close to the last path point of the planned path, if the preview point is always selected as the last path point of the planned path, then when the vehicle moves to the last path point, the vehicle will lose the preview point, resulting in the problem that the steering wheel angle is out of control.

[0198] In view of this, please refer to Figure 10 , in another embodiment, when the vehicle moves close to the end point of the planned path, the preview point is obtained by extending forward from the last path point of the planned path, thereby solving the problem that the preview point is lost when the vehicle moves to the last path point.

[0199] In this embodiment, step 32 includes steps 321 to 325, which are specifically as follows:

[0200] Step 321, determine whether the first reference point is the last path point in the planned path. If the first reference point is the last path point in the planned path, then execute step 322. If the first reference point is not the last path point in the planned path, then execute step 325.

[0201] Step 322, determine whether the shape of the planned path is a straight line. If the shape of the planned path is a straight line, then execute step 323. If the shape of the planned path is not a straight line, then execute step 324.

[0202] Step 323, determine the position point obtained by extending forward a first preset distance L10 along the direction of the first reference point as the preview point.

[0203] Step 324, according to the total number X of path points in the planned path, determine the position point obtained by extending forward a second preset distance L20 along the direction of the first reference point as the preview point; or determine the position point obtained by extending forward a third preset distance L30 along the turning arc where the first reference point is located as the preview point. Wherein, L10 > L20 ≥ L30. Wherein, the direction of the first reference point refers to the direction of the vehicle head when the vehicle reaches the first reference point.

[0204] Step 325, determine the first reference point as the preview point.

[0205] Further, step 324 specifically includes: when X ≥ X3, determine the position point obtained by extending forward a second preset distance L20 along the direction of the first reference point as the preview point. When X < X3, determine the position point obtained by extending forward a third preset distance L30 along the turning arc where the first reference point is located as the preview point.

[0206] Exemplarily, X3 = 5, L10 = 2m, L20 = L30 = 0.8m.

[0207] It should be noted that the values of X3, L10, L20, and L30 can be flexibly selected according to the performance of the vehicle and the distance between two adjacent path points in the planned path, and can be obtained through experiments. The specific values exemplified in the embodiments of the present application are only for illustration to facilitate the understanding of the embodiments provided by the present application, and do not represent that only the values exemplified in the present application can be selected, and should not be regarded as a limitation to the present application.

[0208] In this way, by setting L10 > L20 ≥ L30, when the vehicle moves along a linear planned path and approaches the last path point of the planned path, the position point obtained by extending the last path point forward by a relatively long distance L10 is determined as the preview point, which can avoid the problem of vehicle control oscillation caused by too short preview distance; when the vehicle moves along a unilateral turning arc or S-shaped planned path and approaches the last path point of the planned path, the position point obtained by extending the last path point forward by a relatively short distance L20 or L30 is determined as the preview point, which can avoid the problem of low steering rationality caused by too long preview distance. In addition, according to experience summary, when the total number X of path points in the planned path is less than X3, it is usually the case where the steering wheel angle is set to the maximum (i.e., the steering wheel is turned to the limit). Therefore, when X < X3, the position point obtained by extending the third preset distance L30 forward along the turning arc where the first reference point is located is determined as the preview point, which can ensure that the steering wheel angle of the vehicle remains in the current state, that is, the state where the steering wheel is turned to the limit, when the vehicle reaches the last path point of the planned path.

[0209] Through research, it is found that if the path points in the planned path are directly determined as the preview points, problems such as non-linear, non-smooth, and stuck control of the steering wheel will occur during the process of the vehicle following the path. The specific reasons are as follows: Taking Figure 5b as an example, when the vehicle moves from the first path point towards the second path point, the first following point is the second path point. Correspondingly, the preview point is always the eighth path point and does not move with the movement of the vehicle. When the vehicle reaches the second path point, the first following point becomes the third path point. Correspondingly, the preview point jumps from the eighth path point to the ninth path point. Then, the steering wheel angle of the vehicle will change suddenly, resulting in problems such as non-linear, non-smooth, and stuck control of the steering wheel, which affects the user experience.

[0210] In view of this, please refer to Figures 11-12In another embodiment, when the first reference point is not the last path point in the planned path, the pre-aiming point is determined based on the positional relationship between the vehicle's pose and the first following point, as well as the position of the first reference point, so that the pre-aiming point moves with the movement of the vehicle, thereby solving the problems of non-linear, uneven, and jerky steering wheel control during the vehicle's following path process.

[0211] In this embodiment, step 25 includes steps 251 to 255, and step 32 includes steps 321 to 324 and step 326, as follows:

[0212] Step 251: Determine whether the current following point is the first path point in the planned path. If the current following point is the first path point in the planned path, proceed to step 252. If the current following point is not the first path point in the planned path, proceed to step 253.

[0213] Step 252: Determine the first path point in the planned path that follows the current following point as the first following point.

[0214] Step 253: Determine the current following point as the first following point.

[0215] After completing step 252 or step 253, proceed to step 254.

[0216] Step 254: After determining the first following point, the first path point in the planned path that precedes the current following point is determined as the second following point.

[0217] Step 255: Calculate the first projection distance L1 of the vector from the second following point to the current pose of the vehicle onto the vector from the second following point to the first following point.

[0218] Step 321: Determine whether the first reference point is the last path point in the planned path. If the first reference point is the last path point in the planned path, proceed to step 322. If the first reference point is not the last path point in the planned path, proceed to step 326.

[0219] Step 322: Determine whether the planned path is a straight line. If the planned path is a straight line, proceed to step 323. If the planned path is not a straight line, proceed to step 324.

[0220] Step 323: Determine the position point that extends forward a first preset distance L10 along the orientation of the first reference point as the aiming point.

[0221] Step 324: Based on the total number X of path points in the planned path, determine the position point extending forward a second preset distance L20 along the orientation of the first reference point as the aiming point; or, determine the position point extending forward a third preset distance L30 along the turning arc where the first reference point is located as the aiming point.

[0222] Step 326: Determine the aiming point based on the first projection distance L1.

[0223] Further, please refer to Figure 13 Step 31 includes steps 311 to 315, of which steps 311 to 314 have been described above and will not be repeated here. After completing any one of steps 312, 313, or 314, step 315 is executed, which includes:

[0224] After determining the first reference point, the first path point in the planned path that follows the first reference point is determined as the second reference point.

[0225] As mentioned earlier, if the path points in the planned path are directly determined as the aiming points, problems such as non-linear and uneven steering wheel control and jerking may occur when the vehicle follows the path. Therefore, in this embodiment, after determining the first reference point, the first path point in the planned path after the first reference point is determined as the second reference point. In step 326, the first reference point is specifically determined as the aiming point based on the first projection distance L1; or, the aiming point is obtained by extending along the first reference point to the second reference point. In this way, the aiming point is updated in real time as the vehicle moves, thereby avoiding the situation where the aiming point jumps from one path to another, making the steering wheel control of the vehicle more linear and smoother when following the path.

[0226] Specifically, please refer to Figure 14 Step 326 includes steps 3261 to 3263:

[0227] Step 3261: Determine whether the first projection distance L1 is greater than 0. If the first projection distance L1 is less than or equal to 0, proceed to step 3262; if the first projection distance L1 is greater than 0, proceed to step 3263.

[0228] Step 3262: Determine the first reference point as the pre-aiming point.

[0229] Step 3263: Based on the curvature of the first reference point, the curvature of the second reference point, and the first projection distance L1, the aiming point is obtained by extending along the first reference point to the second reference point.

[0230] It should be noted that when a vehicle is performing path planning, the steering angle of the vehicle when it moves to each path point in the planned path is simulated, so that the curvature of each path point can be determined based on the steering angle.

[0231] It is understandable that the first projection distance L1 being less than 0 means that the angle between the vector from the second following point to the current pose of the vehicle and the vector from the second following point to the first following point is greater than 90 degrees. For example, Figure 5a As shown, at this time, the first path point is located in front of the current direction of the vehicle's movement. The angle between the vector pointing from the second following point (i.e. the first path point) to the current pose of the vehicle and the vector pointing from the second following point to the first following point (i.e. the second path point) is exactly 180 degrees. Therefore, L1 is less than 0.

[0232] To facilitate understanding, the following will be explained in terms of... Figure 5a and Figure 5e This embodiment will be described using the scenario shown as an example. Figure 5aAs shown, the planned path is a straight line. Assuming the vehicle's direction of movement is forward, the total number of path points in the planned path is X = 12, and L = 8. A coordinate system is established with the rear axle center of the vehicle as the origin. The ordinate of the first path point in the planned path is greater than zero and has the smallest absolute value. Therefore, the first path point is the vehicle's current following point. Thus, the first path point after the first path point is the second path point, which is the first following point. The first path point before the first following point, i.e., the first path point, is the second following point. Since the planned path is a straight line, the eighth path point after the first following point (i.e., the second path point), i.e., the tenth path point, is the first reference point. The first path point after the first reference point, i.e., the eleventh path point, is the second reference point. Since L1 < 0, the first reference point (i.e., the tenth path point) is the aiming point. Furthermore, during the vehicle's movement to the first path point, the aiming point remains at the tenth path point. When the vehicle moves between the first and second path points, a coordinate system is established with the rear axle center of the vehicle as the origin. Among the multiple path points in the planned path, the ordinate of the second path point is greater than zero and has the smallest absolute value. Therefore, the second path point is the vehicle's current following point, also known as the first following point. The first path point before the first following point, i.e., the first path point, becomes the second following point. In other words, the first and second following points remain unchanged, and correspondingly, the first and second reference points also remain unchanged; that is, the tenth path point becomes the first reference point, and the eleventh path point becomes the second reference point. Since L1 > 0 at this time, based on the curvature of the first reference point, the curvature of the second reference point, and the first projection distance L1, the aiming point extends from the 10th path point to the 11th path point. Thus, when the vehicle moves to the 1st path point, the aiming point is always the 10th path point. As the vehicle moves from the 1st path point towards the 2nd path point, the position of the aiming point moves with the movement of the vehicle, that is, it moves closer to the 11th path point as the first projection distance L1 increases. Therefore, the aiming point of the vehicle is updated in real time without jumping from one path point to another, making the steering wheel control of the vehicle more linear and smoother when following the path.

[0233] In contrast, if a pre-aiming point is obtained by extending a distance from the current following point to the vehicle's current direction of movement, then when the vehicle moves to before the first path point, the current following point is the first path point. The 8th path point after the current following point (i.e., the first path point), i.e., the 9th path point, is the first reference point. Since L1 < 0 at this time, the first reference point (i.e., the 9th path point) is the pre-aiming point. Then, when the vehicle reaches the first path point, a coordinate system is established with the rear axle center of the vehicle as the origin. The ordinate of the first path point is 0. Therefore, the ordinate of the second path point is greater than zero and has the smallest absolute value, making it the current following point, i.e., the first following point. Correspondingly, the first reference point jumps to the 10th path point. Since L1 = 0 at this time, the first reference point (i.e., the 10th path point) is the pre-aiming point. In other words, during the process of the vehicle moving from the first path point to the first path point, there may be a sudden change in the aiming point. However, in this embodiment, when the current following point is the first path point, the first path point after the first path point is determined as the first following point, and then the aiming point is obtained by extending a certain distance from the first following point in the current direction of the vehicle. This can avoid the problem of sudden changes in the aiming point.

[0234] like Figure 5eAs shown, the planned path is a right-turn arc. Assuming the vehicle's direction of movement is forward, the total number of path points in the planned path is X = 17 > X1 = 15, and M1 = 6. A coordinate system is established with the rear axle center of the vehicle as the origin. The ordinate of the second path point in the planned path is greater than zero and has the smallest absolute value. Therefore, the second path point is the vehicle's current following point, also known as the first following point. The first path point before the current following point, i.e., the first path point, is the second following point. Since the planned path is a right-turn arc and the total number of path points X > X1, the sixth path point after the first following point (i.e., the second path point), i.e., the eighth path point, is the first reference point. The first path point after the first reference point, i.e., the ninth path point, is the second reference point. Since L1 > 0, based on the curvature of the first reference point, the curvature of the second reference point, and the first projection distance L1, the aiming point extends from the 8th path point to the 9th path point. Thus, as the vehicle moves from the first path point towards the second path point, the position of the aiming point moves with the vehicle's movement, that is, it moves closer to the 9th path point as the first projection distance L1 increases. Therefore, the aiming point of the vehicle is updated in real time without jumping from one path point to another, making the steering wheel control of the vehicle more linear and smoother when following the path.

[0235] It should be noted that the example of a right-turn arc-shaped planned path is used here to illustrate the steps for determining the aiming point, and should not be construed as limiting this application. Similarly, in other steps of the embodiments of this application, when the planned path has other shapes, the examples of determining the aiming point are similar, and will not be elaborated here.

[0236] Further, please refer to Figure 15 Step 3263 includes steps 32631 to 32633, as follows:

[0237] Step 32631: Determine whether the curvature of the first reference point and the curvature of the second reference point satisfy any one of three preset conditions. If the curvature of the first reference point and the curvature of the second reference point satisfy any one of the three preset conditions, proceed to step 32632. If the curvature of the first reference point and the curvature of the second reference point do not satisfy any one of the three preset conditions, proceed to step 32633.

[0238] Step 32632: Starting from the first reference point, the position point extending forward by a first projection distance L1 along the direction from the first reference point to the second reference point is determined as the aiming point.

[0239] Step 32633: Using the first reference point as the initial parent node, repeatedly execute the pre-aiming point search step along the arc segment from the first reference point to the second reference point to obtain the pre-aiming point. Here, the parent node refers to the starting point for executing the pre-aiming point search step; that is, the pre-aiming point search begins from the parent node.

[0240] In this embodiment of the application, the three preset conditions include conditions A to C, as follows:

[0241] Condition A: The absolute value of the curvature of the first reference point is less than a preset curvature threshold, and the absolute value of the difference between the curvature of the first reference point and the curvature of the second reference point is less than a first preset curvature difference.

[0242] Condition B: The absolute value of the difference between the curvature of the first reference point and the curvature of the second reference point is greater than the second preset curvature difference.

[0243] Condition C: The curvature of the first reference point has the opposite sign to the curvature of the second reference point.

[0244] For example, the preset curvature threshold is 0.02, the first preset curvature difference is 0.005, and the second preset curvature difference is 0.03.

[0245] It should be noted that the values ​​of the preset curvature threshold, the first preset curvature difference, and the second preset curvature difference can be obtained through experiments. The specific values ​​exemplified in the embodiments of this application are only examples to facilitate understanding of the embodiments provided in this application, and do not mean that only the values ​​exemplified in this application can be used, and should not be regarded as a limitation on this application.

[0246] like Figure 5e As shown, assuming that the curvature of the first reference point (i.e. the 8th path point) and the curvature of the second reference point (i.e. the 9th path point) satisfy condition B, then the aiming point can be obtained by extending L1 forward from the 8th path point along the direction from the 8th path point to the 9th path point.

[0247] It should be noted that when condition A is met, it indicates that the path from the first reference point to the second reference point is approximately a straight line; when condition B is met, it indicates that the curvature change from the first reference point to the second reference point is significant. Extending the path forward by a first projection distance L1 from the first reference point along the straight line pointing to the second reference point yields the pre-aiming point, thus reducing computational load. When condition C is met, it indicates that the curvature of the path points changes from positive to negative, or from negative to positive, meaning there are points with zero curvature in the middle of the path. Since the turning radius corresponding to zero curvature is infinite, this could lead to program errors. In this embodiment, when condition C is met, extending the path forward by a first projection distance L1 from the first reference point along the straight line pointing to the second reference point yields the pre-aiming point, thus avoiding program errors.

[0248] Further, please refer to Figure 16 The pre-aiming point search step includes the following steps:

[0249] Step S1: Starting from the parent node, extend along the arc segment from the first reference point to the second reference point according to a preset step size to obtain candidate position points.

[0250] Step S2: Determine whether the second projection distance L2 of the vector from the first reference point to the candidate position point on the vector from the first reference point to the second reference point is greater than or equal to the first projection distance L1. If the second projection distance L2 of the vector from the first reference point to the candidate position point on the vector from the first reference point to the second reference point is less than the first projection distance L1, then proceed to step S3, and continue with step S1 after step S3. If the second projection distance L2 of the vector from the first reference point to the candidate position point on the vector from the first reference point to the second reference point is greater than or equal to the first projection distance L1, then proceed to step S4.

[0251] Step S3: Use the candidate location point as the new parent node.

[0252] Step S4: Determine the candidate location point as the pre-aiming point and end the pre-aiming point search step.

[0253] In this embodiment, the arc segment from the first reference point to the second reference point is formed by connecting T sub-arc segments sequentially. The length of each of the T sub-arc segments is equal to the preset step size. The curvature of the T sub-arc segments increases or decreases sequentially according to the preset curvature step size Δκ. The curvature κ1 of the first reference point, the curvature κ2 of the second reference point, and the preset curvature step size Δκ have the following relationship:

[0254] κ1+T*Δκ=κ2

[0255] The preset step size can be obtained through experimentation; for example, the preset step size can be 0.05m. For ease of understanding, let's consider... Figure 17 Taking this as an example, the arc segment from the first reference point to the second reference point is formed by connecting sub-arc segments Y1 to Y4 in sequence. The curvature of the points on sub-arc segment Y1 is κ1, the curvature of the points on sub-arc segment Y2 is κ1+Δκ, the curvature of the points on sub-arc segment Y3 is κ1+2*Δκ, the curvature of the points on sub-arc segment Y4 is κ1+3*Δκ, and the second reference point is κ2=κ1+4*Δκ. An example of performing the pre-aiming point search step is as follows: The pre-aiming point search step is performed with the first reference point as the parent node to obtain candidate position point D1. Since the projection distance of the vector pointing from the first reference point to D1 onto the vector pointing from the first reference point to the second reference point is less than L1, the pre-aiming point search step is performed again with D1 as the parent node to obtain candidate position point D2. Since the projection distance of the vector pointing from the first reference point to D1 onto the vector pointing from the first reference point to the second reference point is less than L1, the pre-aiming point search step is performed again with D2 as the parent node to obtain candidate position point D3. Since the projection distance of the vector pointing from the first reference point to D1 onto the vector pointing from the first reference point to the second reference point is greater than L1, D3 is the pre-aiming point.

[0256] Optionally, the pre-aiming point determination method of the pure tracking algorithm may further include: after determining the pre-aiming point, calculating the target steering wheel angle required for the vehicle to move to the pre-aiming point using the control equations of the pure tracking algorithm, so that the EPS controls the steering wheel to follow the target steering wheel angle, thereby realizing the lateral steering control of the vehicle.

[0257] Please see Figure 18 This application also provides a pre-aiming point determination device 60 for a pure tracking algorithm. The pre-aiming point determination device 60 includes a memory 61 and a processor 62. The memory 61 is connected to the processor 62, and the memory 61 stores a computer program 610. When the processor 62 executes the computer program 610, it implements the steps of the pre-aiming point determination method for the pure tracking algorithm described in the above method embodiments.

[0258] It is understandable that the illustration Figure 18 This is merely an example of the pre-aiming point determination device 60 and does not constitute a limitation on the pre-aiming point determination device 60. The pre-aiming point determination device 60 may include more or fewer components than shown, or combine certain components, or different components. For example, the pre-aiming point determination device 60 may also include input / output devices, etc.

[0259] The processor 62 may be a central processing unit (CPU), or it may include other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor may be a microprocessor, or any conventional processor. The processor 62 is the control center of the pre-aiming point determination device 60, connecting various parts of the pure tracking algorithm's pre-aiming point determination device 60 via various interfaces and lines.

[0260] The memory 61 can be used to store the computer program 610. The processor 62 implements various functions of the pre-aiming point determination device 60 by running or executing the computer program 610 stored in the memory 61 and calling data stored in the memory 61. The memory 61 may include an external storage medium or RAM. In addition, the memory 61 may include high-speed random access memory, and may also include non-volatile memory, such as hard disk, RAM, plug-in hard disk, smart media card (SMC), secure digital (SD) card, flash card, at least one disk storage device, flash memory device, or other volatile solid-state storage device.

[0261] The aiming point determination device for the pure tracking algorithm provided in this application determines the current following point based on the relative positional relationship between the vehicle's current pose and multiple path points in the planned path, and obtains a first reference point by extending a corresponding distance forward from the current following point according to the shape of the planned path. Then, the aiming point is determined based on the position of the first reference point. In this way, it is possible to select an appropriate extension distance to obtain the aiming point according to the shape of the planned path, which can avoid the problem of vehicle control oscillation caused by too short an aiming distance and the problem of low steering rationality caused by too long an aiming distance, thereby improving the robustness of the vehicle following the path.

[0262] Please see Figure 19 This application also provides a vehicle 1, including a pre-aiming point determining device 60 and a vehicle body 80 as described in the above embodiments. The pre-aiming point determining device 60 is disposed on the vehicle body 80 and is used to determine a pre-aiming point for the vehicle 1.

[0263] The aiming point determination device for the pure tracking algorithm in this application determines the current following point based on the relative positional relationship between the vehicle's current pose and multiple path points in the planned path, according to the shape of the planned path. Then, based on the shape of the planned path, a first reference point is obtained by extending a corresponding distance forward from the current following point. Finally, the aiming point is determined based on the position of the first reference point. In this way, the aiming point can be obtained by selecting an appropriate extension distance based on the shape of the planned path. This avoids the problem of vehicle control oscillation caused by too short an aiming distance and the problem of low steering rationality caused by too long an aiming distance, thereby improving the robustness of the vehicle following the path.

[0264] This application also provides a computer-readable storage medium having a computer program stored thereon, wherein when the computer program is executed by a processor, it implements the steps of the pre-aiming point determination method of the pure tracking algorithm described in the above method embodiments.

[0265] If the pre-aiming point determination device 60 described in this application is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, all or part of the processes in the methods of the above embodiments of this application can also be implemented by a computer program instructing related hardware. The computer program can be stored in a computer-readable storage medium, and when executed by a processor, it can implement the steps of the various method embodiments described above. The computer program includes computer program code, which can be in the form of source code, object code, executable files, or certain intermediate forms. The computer-readable medium can include: any entity or device capable of carrying the computer program code, a recording medium, a USB flash drive, a portable hard drive, a magnetic disk, an optical disk, a computer memory, a read-only memory (ROM), a random access memory (RAM), an electrical carrier signal, a telecommunication signal, and a software distribution medium, etc. It should be noted that the content contained in the computer-readable medium may be appropriately added to or subtracted from the content as required by the legislation and patent practice in the jurisdiction. For example, in some jurisdictions, according to legislation and patent practice, the computer-readable medium may not include electrical carrier signals and telecommunication signals.

[0266] It will be apparent to those skilled in the art that this application is not limited to the details of the exemplary embodiments described above, and that this application can be implemented in other specific forms without departing from the spirit or essential characteristics of this application. Therefore, the embodiments should be considered exemplary and non-limiting in all respects, and the scope of this application is defined by the appended claims rather than the foregoing description. Thus, all variations falling within the meaning and scope of equivalents of the claims are intended to be embraced within this application. No reference numerals in the claims should be construed as limiting the scope of the claims. Furthermore, it is clear that the word "comprising" does not exclude other units or steps, and the singular does not exclude the plural. Multiple units or devices recited in the apparatus claims may also be implemented by the same unit or device in software or hardware.

[0267] Although embodiments of this application have been shown and described, those skilled in the art will understand that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of this application, the scope of which is defined by the claims and their equivalents.

Claims

1. A method for determining a preview point of a pure pursuit algorithm, applied to a vehicle, characterized in that, include: Obtain the current pose and planned path of the vehicle; wherein the planned path includes multiple waypoints; Based on the relative positional relationship between the vehicle's current pose and multiple path points in the planned path, the current following point is determined from the multiple path points in the planned path; Based on the shape of the planned path, a first reference point is determined from the path points arranged after the current following point in the planned path; and The aiming point is determined based on the position of the first reference point; Wherein, the path point refers to a position point in the planned path; the current following point is the path point among multiple path points in the planned path that is located ahead of the vehicle's current direction of movement and is closest to the vehicle; the first reference point is a path point that extends a certain distance along the planned path towards the vehicle's current direction of movement based on the current following point, the extension distance being related to the shape of the planned path; the aiming point is the current target point determined based on the first reference point, used to calculate the target steering wheel angle of the vehicle; The step of determining the current following point from multiple path points in the planned path based on the relative positional relationship between the vehicle's current pose and multiple path points in the planned path includes: Determine whether the current following point is the first path point in the planned path; If the current following point is the first path point in the planned path, then the first path point in the planned path after the current following point is determined as the first following point; If the current following point is not the first path point in the planned path, then the current following point is determined as the first following point; After determining the first following point, the first path point in the planned path that precedes the current following point is determined as the second following point; and Calculate the first projection distance of the vector from the second following point to the current pose of the vehicle onto the vector from the second following point to the first following point; Determining the aiming point based on the position of the first reference point includes: Determine whether the first reference point is the last path point in the planned path; If the first reference point is the last path point in the planned path, then determine whether the shape of the planned path is a straight line; If the planned path is a straight line, then the position point extending forward a first preset distance along the orientation of the first reference point is determined as the aiming point; If the planned path is not a straight line, then based on the total number of path points in the planned path, a point extending forward a second preset distance along the orientation of the first reference point is determined as the aiming point; or, a point extending forward a third preset distance along the turning arc where the first reference point is located is determined as the aiming point, wherein the first preset distance > the second preset distance ≥ the third preset distance; and If the first reference point is not the last path point in the planned path, then the pre-aiming point is determined based on the first projection distance.

2. The method of pure pursuit algorithm's preview point determination as claimed in claim 1, wherein, The step of determining the current following point from multiple path points in the planned path based on the relative positional relationship between the vehicle's current pose and multiple path points in the planned path includes: A coordinate system is established with the preset part of the vehicle as the origin, and the position coordinates of each path point in the planned path are determined in the coordinate system. Determine whether the vehicle is currently moving forward or backward; If the vehicle's current direction of movement is forward, then the path point with the smallest absolute value of the ordinate that is greater than zero among the multiple path points in the planned path is determined as the current following point; wherein, the extension direction of the ordinate is the direction from the preset part of the vehicle to the front of the vehicle. If the vehicle's current direction of movement is backward, then the path point with the smallest absolute value of its ordinate that is less than zero among the multiple path points in the planned path is determined as the current following point.

3. The method of pure pursuit algorithm's preview point determination as claimed in claim 2, wherein, The planned path includes straight lines, single-sided turning arcs, and S-shapes, wherein the single-sided turning arcs include left-turning arcs and right-turning arcs; Determining a first reference point from the path points arranged after the current following point in the planned path, based on the shape of the planned path, includes: Determine whether the planned path is a straight line, a single-sided curved arc, or an S-shape; If the planned path is a straight line, then based on the relationship between the number of path points arranged after the first following point and L, the Lth path point arranged after the first following point in the planned path is determined as the first reference point, or the last path point in the planned path is determined as the first reference point. If the planned path is a single-sided curved arc, then based on the relationship between the number of path points following the first following point and M, the Mth path point following the first following point is determined as the first reference point; or, the last path point in the planned path is determined as the first reference point. If the planned path is S-shaped, then based on the relationship between the number of path points arranged after the first following point and N, the Nth path point arranged after the first following point in the planned path is determined as the first reference point, or the last path point in the planned path is determined as the first reference point. Where L is the number of the first preset forward path points, M is the number of the second preset forward path points, and N is the number of the third preset forward path points, and L>M>0, L>N>0.

4. The method of pure pursuit algorithm's preview point determination as claimed in claim 3, wherein, The step of determining the Lth path point in the planned path after the first following point as the first reference point, based on the relationship between the number of path points in the planned path following the first following point and L, or determining the last path point in the planned path as the first reference point, includes: Determine whether the number of path points arranged after the first following point in the planned path is greater than L; If the number of path points following the first follower point in the planned path is greater than L, then the Lth path point following the first follower point in the planned path is determined as the first reference point; and If the number of path points arranged after the first following point in the planned path is less than or equal to L, then the last path point in the planned path is determined as the first reference point. The step of determining the Mth path point in the planned path after the first following point as the first reference point, based on the relationship between the number of path points in the planned path following the first following point and M, or determining the last path point in the planned path as the first reference point, includes: Determine whether the number of path points arranged after the first following point in the planned path is greater than M; If the number of path points following the first follower point in the planned path is greater than M, then the Mth path point following the first follower point in the planned path is determined as the first reference point; and If the number of path points arranged after the first following point in the planned path is less than or equal to M, then the last path point in the planned path is determined as the first reference point. The step of determining the Nth path point in the planned path after the first following point as the first reference point, based on the relationship between the number of path points in the planned path following the first following point and N, or determining the last path point in the planned path as the first reference point, includes: Determine whether the number of path points arranged after the first following point in the planned path is greater than N; If the number of path points following the first follower point in the planned path is greater than N, then the Nth path point following the first follower point in the planned path is determined as the first reference point; and If the number of path points arranged after the first following point in the planned path is less than or equal to N, then the last path point in the planned path is determined as the first reference point.

5. The method of pure pursuit algorithm's preview point determination as claimed in claim 3, wherein, The step of determining a first reference point from path points arranged after the current following point in the planned path according to the shape of the planned path further includes: After determining the first reference point, the first path point in the planned path that follows the first reference point is determined as the second reference point; Determining the aiming point based on the first projection distance includes: Determine whether the first projection distance is greater than 0; If the first projection distance is less than or equal to 0, then the first reference point is determined as the pre-aiming point; and If the first projection distance is greater than 0, then the pre-aiming point is obtained by extending along the first reference point to the second reference point based on the curvature of the first reference point, the curvature of the second reference point, and the first projection distance.

6. The method of pure pursuit algorithm's preview point determination as claimed in claim 5, wherein, The step of obtaining the pre-aiming point by extending along the first reference point to the second reference point based on the curvature of the first reference point, the curvature of the second reference point, and the first projection distance includes: Determine whether the curvature of the first reference point and the curvature of the second reference point satisfy any one of three preset conditions; If the curvature of the first reference point and the curvature of the second reference point satisfy any one of the three preset conditions, then the position point extending forward by a first projection distance from the first reference point along the direction pointing from the first reference point to the second reference point is determined as the aiming point; If the curvature of the first reference point and the curvature of the second reference point do not satisfy any of the three preset conditions, then the aiming point search step is repeated along the arc segment from the first reference point to the second reference point, with the first reference point as the initial parent node, to obtain the aiming point. Here, the parent node refers to the starting point for executing the aiming point search step. The pre-aiming point search step includes: Starting from the parent node, extend along the arc segment from the first reference point to the second reference point according to a preset step size to obtain candidate position points; Determine whether the second projection distance of the vector from the first reference point to the candidate position point on the vector from the first reference point to the second reference point is greater than or equal to the first projection distance; If the second projection distance of the vector from the first reference point to the candidate location point on the vector from the first reference point to the second reference point is less than the first projection distance, then the pre-aiming point search step is continued with the candidate location point as the new parent node; and If the second projection distance of the vector from the first reference point to the candidate position point on the vector from the first reference point to the second reference point is greater than or equal to the first projection distance, then the candidate position point is determined as the pre-aiming point, and the pre-aiming point search step ends. The three preset conditions include: The absolute value of the curvature of the first reference point is less than a preset curvature threshold, and the absolute value of the difference between the curvature of the first reference point and the curvature of the second reference point is less than a first preset curvature difference. The absolute value of the difference between the curvature of the first reference point and the curvature of the second reference point is greater than the second preset curvature difference; and The curvature of the first reference point has the opposite sign to the curvature of the second reference point.

7. The method of pure pursuit algorithm's preview point determination as claimed in claim 3, wherein, The values ​​of M include M1, M2, and M3; where M1 > M2 > M3. The step of determining the Mth path point in the planned path that follows the first following point as the first reference point includes: Determine the relationship between the total number of path points in the planned path and a first preset threshold and a second preset threshold; wherein the first preset threshold is greater than the second preset threshold; If the total number of path points in the planned path is greater than the first preset number threshold, then the M1th path point in the planned path that follows the first following point is determined as the first reference point. If the total number of path points in the planned path is less than or equal to the first preset number threshold and greater than the second preset number threshold, then the M2th path point in the planned path after the first following point is determined as the first reference point. If the total number of path points in the planned path is less than or equal to the second preset threshold, then the M3th path point in the planned path that follows the first following point is determined as the first reference point.

8. A pre-impact point determination device for a pure pursuit algorithm, characterized by The aiming point determination device for the pure tracking algorithm includes a memory and a processor. The memory is connected to the processor, and the memory stores a computer program. When the computer program is executed by the processor, it implements the aiming point determination method for the pure tracking algorithm as described in any one of claims 1 to 7.

9. A vehicle, characterized in that, The vehicles include: Vehicle body; and The aiming point determination device for the pure tracking algorithm as described in claim 8, wherein the aiming point determination device for the pure tracking algorithm is disposed on the vehicle body, and the aiming point determination device for the pure tracking algorithm is used to determine the aiming point for the vehicle, thereby controlling the movement of the vehicle based on the aiming point.

10. A computer-readable storage medium having stored thereon a computer program, characterized in that, When the computer program is executed by the processor, it implements the pre-aiming point determination method of the pure tracking algorithm as described in any one of claims 1 to 7.

Images