A motion preview method for target pose following in automobile field test

Abstract

The application is suitable for the field of automatic driving technology, and provides a motion preview method for target pose following in vehicle field test, which comprises the following steps: step 1, task preview; step 2, task segmentation; and step 3, motion preview. The method takes a segment of motion with constant curvature of a vehicle driving track as a primitive motion, establishes a prediction model of a plurality of segments of the primitive motion in succession and a target function considering position deviation and direction deviation, obtains lateral acceleration of the target through optimization iteration design variable, and realizes that the position and direction of the predicted track at the target point simultaneously meet the requirements of the target track. Meanwhile, the constraints of the maneuverability and lateral stability of the vehicle are considered, so that the feasibility of the preview result is ensured. In addition, the initial value of the optimization algorithm is obtained through the preview and segmentation of the target test track, so that the solving speed and calculation efficiency are improved.

Description

Technical Field

[0001] This invention belongs to the field of autonomous driving technology, and in particular relates to a motion pre-aiming method for target pose following in vehicle track testing. Background Technology

[0002] Automotive track testing is a crucial part of the automotive research, development, production, and certification process. Track tests, such as steady-state circular tests and kink tests, require the vehicle to follow a given trajectory to complete the test.

[0003] Current trajectory following methods for autonomous vehicles mainly include those based on control theory, such as PID control, LQR linear quadratic regulator control, and MPC model predictive control; and those based on aiming-following theory, such as the OPA optimal aiming acceleration model.

[0004] Methods based on control theory determine the control input based on the deviation between the vehicle's actual position and the reference trajectory, continuously refining the control input to reduce error. However, they primarily focus on position following and rarely consider whether the vehicle's direction at the target position matches the expected direction. Methods based on preview-following theory first preview the vehicle's optimal acceleration and then use control theory to follow the preview result. The preview process employs a simple, single-segment motion model, such as a second-order Taylor expansion of rigid body planar kinematics. The preview result struggles to simultaneously match the vehicle's direction and position requirements for the target trajectory. Summary of the Invention

[0005] The purpose of this invention is to provide a motion pre-aiming method for target pose tracking in automotive track testing, aiming to solve the problems mentioned in the background art.

[0006] The present invention is implemented as follows: a motion pre-aiming method for target pose tracking in automotive track testing includes the following steps:

[0007] Step 1: Mission Pre-aiming;

[0008] The target trajectory point coordinates of the vehicle track test conditions are used as input, and the output is the trajectory point coordinates that the current motion preview needs to follow.

[0009] Step 2: Task segmentation;

[0010] The trajectory that needs to be followed is segmented according to curvature, that is, the driving task is divided into straight segments and turning segments, and the segmentation results are used as the initial conditions for motion pre-aiming;

[0011] The current motion preview takes the coordinates of the trajectory points to be followed as input and outputs the segmented results, including the number of segments, the length of each segment, and the magnitude and sign of the average curvature of each segment. The curvature of the straight segment is 0, and the curvature of the turning segment is positive for left turns and negative for right turns.

[0012] Step 3: Pre-aiming during movement;

[0013] Determine the lateral acceleration of the vehicle target to ensure that the vehicle can simultaneously meet the position and orientation requirements at the pre-aiming point; take the coordinates of the trajectory point to be followed, the task segmentation results, and the motion state of the vehicle as input, and output the lateral acceleration for pre-aiming.

[0014] A further technical solution, the specific steps of step 1 are as follows:

[0015] The aiming distance is determined based on the current vehicle speed and aiming time, using the following formula:

[0016] ;

[0017] In the formula, Indicates the aiming distance. Indicates the current vehicle speed. Indicates the aiming time.

[0018] The formula for transforming the coordinates of the target trajectory points in the vehicle test track condition to the vehicle coordinate system is as follows:

[0019] ;

[0020] In the formula, Represents the global coordinates of the target trajectory point. This represents the coordinates of the target trajectory point in the vehicle coordinate system. This represents the car's current global coordinates. Indicates the car's current heading angle;

[0021] The x-coordinate of the target trajectory point in the vehicle coordinate system is compared with the pre-aiming distance. The trajectory point with a x-coordinate greater than 0 and less than the pre-aiming distance is taken as the current target to be followed, as shown in the following formula:

[0022] ;

[0023] In the formula, Indicates the trajectory point that needs to be followed. The set, This represents the x-coordinate of the target trajectory point in the vehicle coordinate system.

[0024] A further technical solution, the specific steps of step 2 are as follows:

[0025] Polynomial fitting is performed on the trajectory points to be followed to obtain the function that forms the curve of the trajectory points. The curvature formulas at each trajectory point are obtained from the curvature formula as follows:

[0026] ;

[0027] In the formula, Represents trajectory points curvature at that point and Representing functions respectively At point The values ​​of the first and second derivatives at the given point;

[0028] Set a threshold value for curvature segmentation. Based on the magnitude and sign of curvature at each trajectory point, divide the target trajectory into several straight line segments, left-turn segments, and right-turn segments, as shown in the following formula:

[0029] ;

[0030] In the formula, The threshold value representing the curvature segment. Let represent the set of the i-th line segments. Let j represent the set of left-turn segments. This represents the set of the k-th right-turn segments;

[0031] The number of segments is the sum of the number of straight segments, left-turn segments, and right-turn segments; the length of each segment is the sum of the Euclidean distances of the trajectory points contained within the segment; the average curvature of each segment is the average curvature of the trajectory points contained within the segment, as shown in the following formula:

[0032] ;

[0033] In the formula, Indicates the number of segments, These represent the number of straight segments, the number of left-turn segments, and the number of right-turn segments, respectively. Indicates the first The length of the segment Indicates the first Section 1 The coordinates of the trajectory points Indicates the first Section 1 The coordinates of the trajectory points Indicates the first The number of trajectory points in the segment. Indicates the first The average curvature of the segment, Indicates the first Section 1 Curvature at each trajectory point.

[0034] A further technical solution, the specific steps of step 3 are as follows:

[0035] Step 3.1: Construct the objective function, while following the position and direction of the target trajectory, normalize the position and direction deviations between the predicted trajectory and the target trajectory, use the sum of squares of the normalization results as the objective function, and use an optimization algorithm to find the solution that minimizes the objective function;

[0036] The predicted trajectory is discretized into several points. The projected distances from these points to the target trajectory and the heading angles at the projected points are calculated. The projected distances are used as the positional deviations between the predicted and target trajectories, and the deviations between the heading angles at the projected points and the heading angles at the predicted trajectory points are used as the directional deviations. The objective function formula is as follows:

[0037] ;

[0038] In the formula, This represents the objective function that takes the minimum value. This represents the number of discrete points on the predicted trajectory. This represents the normalization coefficient for positional deviation. This represents the normalization coefficient for directional deviation. Indicates the first One positional deviation, Indicates the first One directional deviation;

[0039] Step 3.2: Introduce constraints. Considering the constraints on vehicle maneuverability, the curvature of the pre-aimed trajectory should meet the constraint of the vehicle's minimum turning radius; considering the requirements of vehicle lateral stability, the lateral acceleration of the pre-aimed trajectory should be less than the lateral acceleration of vehicle instability; considering the pose following requirements of the predicted trajectory at the endpoint, the position and direction deviations between the predicted trajectory and the target trajectory at the endpoint should be sufficiently small, as shown in the following formula:

[0040] ;

[0041] In the formula, Indicates the curvature of the pre-aimed trajectory. This indicates the minimum turning radius of the car. This indicates the lateral acceleration during pre-aiming. This indicates the maximum lateral acceleration that a car can achieve. This indicates the positional deviation between the predicted trajectory and the target trajectory at the endpoint. This represents the threshold value for positional deviation at the endpoint. This indicates the directional deviation between the predicted trajectory and the target trajectory at the endpoint. Indicates the directional deviation threshold at the endpoint;

[0042] Step 3.3: Implement the prediction model. A segment of the car's trajectory with constant curvature is used as the basic motion. The prediction model is constructed by connecting multiple basic motion segments. The arc length and curvature of each segment are used as design variables for the optimization method, and the pose at the endpoint of each segment is used as the dependent variable. Iterative optimization is then performed. The prediction formula for a single basic motion segment is as follows:

[0043] straight segment ,have:

[0044] ;

[0045] Left or right turn section ,have:

[0046] ;

[0047] In the formula, This represents the vehicle pose predicted by a single-segment primitive motion. This represents the initial pose during the prediction process. The curvature representing a single segment of motion. Indicates the length of the arc in a single segment of motion;

[0048] The predicted trajectory is obtained by continuating single-segment elementary motions. Its pose at the endpoint is a function of the initial pose, the number of segments, and the curvature and arc length of each segment, as expressed below:

[0049] ;

[0050] In the formula, Indicates the pose at the endpoint. This represents the curvature of the first segment of motion. This represents the arc length of the first single-segment motion. This represents the curvature of the second single-segment motion. This represents the arc length of the second single-segment motion. Denotes the curvature of the nth segment of motion. This represents the arc length of the nth segment of a single motion. Indicates the number of segments;

[0051] Step 3.4: Solve using an optimization algorithm. Within the constraints, iteratively optimize the design variables, using the SQP sequential quadratic programming method to find a set of design variables that minimize the objective function. Multiply the first segment of the pre-aiming curvature in the design variables by the square of the vehicle speed to obtain the pre-aiming result, i.e., the expected lateral acceleration of the vehicle, as shown in the following formula:

[0052] ;

[0053] In the formula, This indicates the lateral acceleration during pre-aiming. It represents the curvature of the first segment of motion.

[0054] Another objective of this invention is to provide a motion pre-aiming system for target pose tracking in automotive track testing, based on the aforementioned motion pre-aiming method, comprising:

[0055] The task aiming module takes the coordinates of the target trajectory point in the vehicle test site as input and outputs the coordinates of the trajectory point that the current motion aiming needs to follow.

[0056] The task segmentation module is used to segment the current trajectory to be followed according to curvature, that is, to divide the driving task into straight segments and turning segments. The segmentation result is used as the initial condition for motion preview. Taking the coordinates of the trajectory points to be followed by the current motion preview as input, the output of the segmentation result includes the number of segments, the length of each segment, and the magnitude and sign of the average curvature of each segment. Among them, the curvature of the straight segment is 0, and the curvature of the turning segment is positive for left turns and negative for right turns.

[0057] The motion preview module is used to determine the lateral acceleration of the vehicle target, ensuring that the vehicle can simultaneously meet the position and orientation requirements at the preview point; it takes the coordinates of the trajectory point to be followed, the task segmentation results, and the motion state of the vehicle as input, and outputs the lateral acceleration of the preview.

[0058] This invention provides a motion pre-aiming method for target pose tracking in vehicle track testing. Addressing the challenge of current motion pre-aiming methods simultaneously tracking the direction and position of a target trajectory, this method employs a multi-segment elementary motion continuation solution. A segment of the vehicle's trajectory with constant curvature is used as the elementary motion. A prediction model for multi-segment elementary motion continuation and an objective function considering position and direction deviations are established. The lateral acceleration of the target is obtained through iterative optimization of variables, ensuring that the predicted trajectory's position and direction at the target point simultaneously meet the target trajectory requirements. Constraints on vehicle maneuverability and lateral stability are also considered, guaranteeing the feasibility of the pre-aiming results. Furthermore, by pre-aiming and segmenting the target test trajectory, initial values ​​for the optimization algorithm are obtained, improving the solution speed and computational efficiency. Attached Figure Description

[0059] Figure 1 This is a functional block diagram of a motion pre-aiming method for target pose tracking in automotive track testing, provided by an embodiment of the present invention.

[0060] Figure 2 This is a functional block diagram of motion pre-aiming in a motion pre-aiming method for target pose tracking in automotive track testing, provided in an embodiment of the present invention. Detailed Implementation

[0061] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0062] The specific implementation of the present invention will be described in detail below with reference to specific embodiments.

[0063] like Figure 1 and Figure 2 As shown, a motion pre-aiming method for target pose tracking in automotive track testing, provided by an embodiment of the present invention, includes the following steps:

[0064] Step 1: Mission Pre-aiming;

[0065] The system takes the coordinates of the target trajectory points in the vehicle track test conditions as input and outputs the coordinates of the trajectory points that the current motion preview needs to follow. Specifically:

[0066] The aiming distance is determined based on the current vehicle speed and aiming time, using the following formula:

[0067] ;

[0068] In the formula, Indicates the aiming distance. Indicates the current vehicle speed. Indicates the aiming time.

[0069] The formula for transforming the coordinates of the target trajectory points in the vehicle test track condition to the vehicle coordinate system is as follows:

[0070] ;

[0071] In the formula, Represents the global coordinates of the target trajectory point. This represents the coordinates of the target trajectory point in the vehicle coordinate system. This represents the car's current global coordinates. This indicates the car's current heading angle.

[0072] The x-coordinate of the target trajectory point in the vehicle coordinate system is compared with the pre-aiming distance. The trajectory point with a x-coordinate greater than 0 and less than the pre-aiming distance is taken as the current target to be followed, as shown in the following formula:

[0073] ;

[0074] In the formula, Indicates the trajectory point that needs to be followed. The set, This represents the x-coordinate of the target trajectory point in the vehicle coordinate system.

[0075] Step 2: Task segmentation;

[0076] The trajectory to be followed is segmented according to curvature, that is, the driving task is divided into straight segments and turning segments, and the segmentation results are used as the initial conditions for motion preview.

[0077] The program takes the coordinates of the trajectory points to be followed by the current motion preview as input and outputs the segmented results, including the number of segments, the length of each segment, and the magnitude and sign of the average curvature of each segment (0 for straight segments, positive for left turns and negative for right turns). Specifically:

[0078] Polynomial fitting is performed on the trajectory points to be followed to obtain the function that forms the curve of the trajectory points. The curvature formulas at each trajectory point are obtained from the curvature formula as follows:

[0079] ;

[0080] In the formula, Represents trajectory points curvature at that point and Representing functions respectively At point The values ​​of the first and second derivatives at the given point.

[0081] Set a threshold value for curvature segmentation. Based on the magnitude and sign of curvature at each trajectory point, divide the target trajectory into several straight line segments, left-turn segments, and right-turn segments, as shown in the following formula:

[0082] ;

[0083] In the formula, The threshold value representing the curvature segment. Let represent the set of the i-th line segments. Let j represent the set of left-turn segments. Let represent the set of the k-th right-turn segments.

[0084] The number of segments is the sum of the number of straight segments, left-turn segments, and right-turn segments; the length of each segment is the sum of the Euclidean distances of the trajectory points contained within the segment; the average curvature of each segment is the average curvature of the trajectory points contained within the segment, as shown in the following formula:

[0085] ;

[0086] In the formula, Indicates the number of segments, These represent the number of straight segments, the number of left-turn segments, and the number of right-turn segments, respectively. Indicates the first The length of the segment Indicates the first Section 1 The coordinates of the trajectory points Indicates the first Section 1 The coordinates of the trajectory points Indicates the first The number of trajectory points in the segment. Indicates the first The average curvature of the segment, Indicates the first Section 1 Curvature at each trajectory point.

[0087] Step 3: Pre-aiming during movement;

[0088] The lateral acceleration of the vehicle target is determined to ensure that the vehicle simultaneously meets the position and orientation requirements at the pre-aiming point. The pre-aiming process considers constraints such as the vehicle's current motion state, maneuverability constraints, and lateral stability constraints. Using the coordinates of the trajectory point to be followed, the task segmentation results, and the vehicle's motion state as input, the output is the pre-aiming lateral acceleration. Specifically:

[0089] Step 3.1: Construct the objective function, while following the position and direction of the target trajectory, normalize the position and direction deviations between the predicted trajectory and the target trajectory, and use the sum of squares of the normalization results as the objective function. Find the solution that minimizes the objective function through an optimization algorithm.

[0090] The predicted trajectory is discretized into several points. The projected distances from these points to the target trajectory and the heading angles at the projected points are calculated. The projected distances are used as the positional deviations between the predicted and target trajectories, and the deviations between the heading angles at the projected points and the heading angles at the predicted trajectory points are used as the directional deviations. The objective function formula is as follows:

[0091] ;

[0092] In the formula, This represents the objective function that takes the minimum value. This represents the number of discrete points on the predicted trajectory. This represents the normalization coefficient for positional deviation. This represents the normalization coefficient for directional deviation. Indicates the first One positional deviation, Indicates the first One directional deviation.

[0093] Step 3.2: Introduce constraints. Considering the constraints on vehicle maneuverability, the curvature of the preview trajectory should satisfy the constraint of the vehicle's minimum turning radius. Considering the requirements for vehicle lateral stability, the lateral acceleration of the preview should be less than the lateral acceleration that causes vehicle instability. Considering the pose following requirements of the predicted trajectory at the endpoint, the position and direction deviations between the predicted trajectory and the target trajectory at the endpoint should be sufficiently small, as shown in the following formula:

[0094] ;

[0095] In the formula, Indicates the curvature of the pre-aimed trajectory. This indicates the minimum turning radius of the car. This indicates the lateral acceleration during pre-aiming. This indicates the maximum lateral acceleration that a car can achieve. This indicates the positional deviation between the predicted trajectory and the target trajectory at the endpoint. This represents the threshold value for positional deviation at the endpoint. This indicates the directional deviation between the predicted trajectory and the target trajectory at the endpoint. This represents the directional deviation threshold at the endpoint.

[0096] Step 3.3: Implement the prediction model. A segment of the car's trajectory with constant curvature is used as the primitive motion. The prediction model is constructed by connecting multiple primitive motion segments. The arc length and curvature of each segment are used as design variables for the optimization method, and the pose (position and orientation) at the endpoint of each segment is used as the dependent variable. Iterative optimization is then performed. The prediction formula for a single primitive motion segment is as follows:

[0097] straight segment ,have:

[0098] ;

[0099] Left or right turn section ,have:

[0100] ;

[0101] In the formula, This represents the vehicle pose predicted by a single-segment primitive motion. This represents the initial pose during the prediction process. The curvature representing a single segment of motion. It represents the length of the arc of a single-segment motion.

[0102] The predicted trajectory is obtained by continuating single-segment elementary motions. Its pose at the endpoint is a function of the initial pose, the number of segments, and the curvature and arc length of each segment, as expressed below:

[0103] ;

[0104] In the formula, Indicates the pose at the endpoint. This represents the curvature of the first segment of motion. This represents the arc length of the first single-segment motion. This represents the curvature of the second single-segment motion. This represents the arc length of the second single-segment motion. Denotes the curvature of the nth segment of motion. This represents the arc length of the nth segment of a single motion. Indicates the number of segments.

[0105] Step 3.4: Solve using an optimization algorithm. Within the constraints, iteratively optimize the design variables, using the SQP sequential quadratic programming method to find a set of design variables that minimize the objective function. Multiply the first segment of the target curvature in the design variables by the square of the vehicle speed to obtain the target result, i.e., the expected lateral acceleration of the vehicle, as shown in the following formula:

[0106] ;

[0107] In the formula, This indicates the lateral acceleration during pre-aiming. It represents the curvature of the first segment of motion.

[0108] An embodiment of the present invention provides a motion pre-aiming system for target pose tracking in automotive track testing, based on the above-described motion pre-aiming method, comprising:

[0109] The task aiming module takes the coordinates of the target trajectory point in the vehicle test site as input and outputs the coordinates of the trajectory point that the current motion aiming needs to follow.

[0110] The task segmentation module is used to segment the trajectory to be followed according to curvature, that is, to divide the driving task into straight segments and turning segments. The segmentation result is used as the initial condition for motion preview. The module takes the coordinates of the trajectory points to be followed by the current motion preview as input and outputs the segmentation result, including the number of segments, the length of each segment, and the magnitude and sign of the average curvature of each segment. The curvature of the straight segment is 0, and the curvature of the turning segment is positive for left turns and negative for right turns.

[0111] The motion preview module determines the lateral acceleration of the vehicle target, ensuring that the vehicle simultaneously meets the position and orientation requirements at the preview point. The preview process considers constraints such as the vehicle's current motion state, maneuverability constraints, and lateral stability constraints. It takes the coordinates of the trajectory point to be followed, the task segmentation results, and the vehicle's motion state as input, and outputs the previewed lateral acceleration.

[0112] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A motion pre-aiming method for target pose tracking in automotive track testing, characterized in that, Includes the following steps: Step 1: Mission Pre-aiming; The target trajectory point coordinates of the vehicle track test conditions are used as input, and the output is the trajectory point coordinates that the current motion preview needs to follow. Step 2: Task segmentation; The trajectory that needs to be followed is segmented according to curvature, that is, the driving task is divided into straight segments and turning segments, and the segmentation results are used as the initial conditions for motion pre-aiming; The current motion preview takes the coordinates of the trajectory points to be followed as input and outputs the segmented results, including the number of segments, the length of each segment, and the magnitude and sign of the average curvature of each segment. The curvature of the straight segment is 0, and the curvature of the turning segment is positive for left turns and negative for right turns. Step 3: Pre-aiming during movement; Determine the lateral acceleration of the vehicle target to ensure that the vehicle can simultaneously meet the position and orientation requirements at the pre-aiming point; take the coordinates of the trajectory point to be followed, the task segmentation results, and the motion state of the vehicle as input, and output the lateral acceleration for pre-aiming.

2. The motion pre-aiming method for target pose tracking in automotive track testing according to claim 1, characterized in that, The specific steps of step 1 are as follows: The aiming distance is determined based on the current vehicle speed and aiming time, using the following formula: ； In the formula, Indicates the aiming distance. Indicates the current vehicle speed. Indicates the aiming time; The formula for transforming the coordinates of the target trajectory points in the vehicle test track condition to the vehicle coordinate system is as follows: ； In the formula, Represents the global coordinates of the target trajectory point. This represents the coordinates of the target trajectory point in the vehicle coordinate system. This represents the car's current global coordinates. Indicates the car's current heading angle; The x-coordinate of the target trajectory point in the vehicle coordinate system is compared with the pre-aiming distance. The trajectory point with a x-coordinate greater than 0 and less than the pre-aiming distance is taken as the current target to be followed, as shown in the following formula: ； In the formula, Indicates the trajectory point that needs to be followed. The set, This represents the x-coordinate of the target trajectory point in the vehicle coordinate system.

3. The motion pre-aiming method for target pose tracking in automotive track testing according to claim 2, characterized in that, The specific steps of step 2 are as follows: Polynomial fitting is performed on the trajectory points to be followed to obtain the function that forms the curve of the trajectory points. The curvature formulas at each trajectory point are obtained from the curvature formula as follows: ； In the formula, Represents trajectory points curvature at that point and Representing functions respectively At point The values ​​of the first and second derivatives at the given point; Set a threshold value for curvature segmentation. Based on the magnitude and sign of curvature at each trajectory point, divide the target trajectory into several straight line segments, left-turn segments, and right-turn segments, as shown in the following formula: ； In the formula, The threshold value representing the curvature segment. Let represent the set of the i-th line segments. Let j represent the set of left-turn segments. This represents the set of the k-th right-turn segments; The number of segments is the sum of the number of straight segments, left-turn segments, and right-turn segments; the length of each segment is the sum of the Euclidean distances of the trajectory points contained within the segment; the average curvature of each segment is the average curvature of the trajectory points contained within the segment, as shown in the following formula: ； In the formula, Represents the number of segments, These represent the number of straight segments, the number of left-turn segments, and the number of right-turn segments, respectively. Indicates the first The length of the segment Indicates the first Section 1 The coordinates of the trajectory points Indicates the first Section 1 The coordinates of the trajectory points Indicates the first The number of trajectory points in the segment. Indicates the first The average curvature of the segment, Indicates the first Section 1 Curvature at each trajectory point.

4. The motion pre-aiming method for target pose tracking in automotive track testing according to claim 3, characterized in that, The specific steps of step 3 are as follows: Step 3.1: Construct the objective function, while following the position and direction of the target trajectory, normalize the position and direction deviations between the predicted trajectory and the target trajectory, use the sum of squares of the normalization results as the objective function, and use an optimization algorithm to find the solution that minimizes the objective function; The predicted trajectory is discretized into several points. The projected distances from these points to the target trajectory and the heading angles at the projected points are calculated. The projected distances are used as the positional deviations between the predicted and target trajectories, and the deviations between the heading angles at the projected points and the heading angles at the predicted trajectory points are used as the directional deviations. The objective function formula is as follows: ； In the formula, This represents the objective function that takes the minimum value. This represents the number of discrete points on the predicted trajectory. This represents the normalization coefficient for positional deviation. This represents the normalization coefficient for directional deviation. Indicates the first One positional deviation Indicates the first One directional deviation; Step 3.2: Introduce constraints. Considering the constraints on vehicle maneuverability, the curvature of the pre-aimed trajectory should meet the constraint of the vehicle's minimum turning radius; considering the requirements of vehicle lateral stability, the lateral acceleration of the pre-aimed trajectory should be less than the lateral acceleration of vehicle instability; considering the pose following requirements of the predicted trajectory at the endpoint, the position and direction deviations between the predicted trajectory and the target trajectory at the endpoint should be sufficiently small, as shown in the following formula: ； In the formula, Indicates the curvature of the pre-aimed trajectory. This indicates the minimum turning radius of the car. This indicates the lateral acceleration during pre-aiming. This indicates the maximum lateral acceleration that a car can achieve. This indicates the positional deviation between the predicted trajectory and the target trajectory at the endpoint. This represents the positional deviation threshold at the endpoint. This indicates the directional deviation between the predicted trajectory and the target trajectory at the endpoint. Indicates the directional deviation threshold at the endpoint; Step 3.3: Implement the prediction model. A segment of the car's trajectory with constant curvature is used as the basic motion. The prediction model is constructed by connecting multiple basic motion segments. The arc length and curvature of each segment are used as design variables for the optimization method, and the pose at the endpoint of each segment is used as the dependent variable. Iterative optimization is then performed. The prediction formula for a single basic motion segment is as follows: straight segment ,have: ； Left or right turn section ,have: ； In the formula, This represents the vehicle pose predicted by a single-segment primitive motion. This represents the initial pose during the prediction process. The curvature representing a single segment of motion. Indicates the length of the arc in a single segment of motion; The predicted trajectory is obtained by continuating single-segment elementary motions. Its pose at the endpoint is a function of the initial pose, the number of segments, and the curvature and arc length of each segment, as expressed below: ； In the formula, Indicates the pose at the endpoint. This represents the curvature of the first segment of motion. This represents the arc length of the first single-segment motion. This represents the curvature of the second single-segment motion. This represents the arc length of the second single-segment motion. Denotes the curvature of the nth segment of motion. This represents the arc length of the nth segment of a single motion. Indicates the number of segments; Step 3.4: Solve using an optimization algorithm. Within the constraints, iteratively optimize the design variables, using the SQP sequential quadratic programming method to find a set of design variables that minimize the objective function. Multiply the first segment of the pre-aiming curvature in the design variables by the square of the vehicle speed to obtain the pre-aiming result, i.e., the expected lateral acceleration of the vehicle, as shown in the following formula: ； In the formula, This indicates the lateral acceleration during pre-aiming. It represents the curvature of the first segment of motion.

5. A motion pre-aiming system for target pose tracking in automotive track testing, based on the motion pre-aiming method for target pose tracking in automotive track testing according to any one of claims 1-4, characterized in that, include: The task aiming module takes the coordinates of the target trajectory point in the vehicle test site as input and outputs the coordinates of the trajectory point that the current motion aiming needs to follow. The task segmentation module is used to segment the trajectory that needs to be followed according to curvature, that is, to divide the driving task into straight segments and turning segments. The segmentation results are used as the initial conditions for motion preview. The current motion preview takes the coordinates of the trajectory points to be followed as input and outputs the segmented results, including the number of segments, the length of each segment, and the magnitude and sign of the average curvature of each segment. The motion pre-aiming module is used to determine the lateral acceleration of the vehicle target, ensuring that the vehicle can simultaneously meet the position and orientation requirements at the pre-aiming point; The system takes the coordinates of the trajectory points to be followed, the task segmentation results, and the motion state of the car as inputs and outputs the lateral acceleration for aiming.

Images