Low-speed unmanned vehicle trajectory tracking and motion control method and system

Abstract

The application relates to the technical field of motion control, and discloses a low-speed unmanned vehicle trajectory tracking and motion control method and system, which comprises the following steps: step 1, path geometry modeling is carried out to generate a reference path table; step 2, the initial heading of the vehicle is aligned, a constant table is loaded, and a vehicle attitude is formed; step 3, nearest point matching is carried out to obtain a lateral error, a heading error and a current path curvature; step 4, non-complete constraint projection is executed to calculate an instant curvature and form a curvature consistency constraint; step 5, steering static integrated solving and actuator static reverse mapping are executed to obtain a steering actuator input; step 6, a curvature constraint and a steering dynamic constraint speed upper limit are calculated, and a current actual speed is obtained; and step 7, bias correction and parameter updating are carried out to realize deterministic reset. The application realizes high-precision trajectory tracking and stable motion control of a low-speed unmanned vehicle under the working conditions of narrow roads and frequent starting and stopping.

Description

Technical Field

[0001] This invention belongs to the field of motion control technology, specifically relating to a method and system for low-speed unmanned vehicle trajectory tracking and motion control. Background Technology

[0002] Low-speed unmanned vehicles are mainly deployed in restricted scenarios such as park logistics, factory handling, sanitation operations, and parking lot shuttles. These scenarios are characterized by low vehicle speed, narrow space, small turning radius, and many environmental obstructions, which place high demands on trajectory tracking accuracy, steering smoothness, and long-term operational stability. In existing technologies, geometric path tracking algorithms such as pure tracking are mostly used, or integrated control schemes combining model predictive control and extended Kalman filtering can achieve good results under medium and high speed conditions. However, under low-speed, small-radius curves, and frequent start-stop conditions, they are easily affected by factors such as actuator dead zone, inertial measurement unit zero drift, and decreased observability due to low vehicle speed, resulting in the gradual accumulation of path tracking errors.

[0003] On the other hand, existing solutions typically design path geometry description, steering control, and speed planning in a fragmented manner: paths are often stored as discrete points or simple polylines, without explicitly labeling geometric information such as curvature within the same structure; steering control outputs the steering angle based solely on instantaneous lateral and heading errors, making it difficult to consider actuator dead zones, proportional coefficient variations, and input rate of change constraints; longitudinal speed planning often relies on empirically given speed curvature or speed segment mapping, lacking integrated constraints with steering execution capability, maximum lateral acceleration, and maximum yaw rate. In weak positioning or occlusion environments, the system usually relies on high-precision positioning or complex filtering algorithms for compensation, resulting in complex implementation, parameter sensitivity, high requirements for onboard computing resources and sensor configuration, and a lack of a mechanism for organized, self-consistent correction of the inertial measurement unit and steering actuator at zero or near-zero speeds. Summary of the Invention

[0004] This invention provides a method and system for trajectory tracking and motion control of low-speed unmanned vehicles, which solves the technical problems in related technologies, such as the disconnect between path geometry information, non-ideal characteristics of actuators and vehicle kinematic constraints, leading to the accumulation of trajectory tracking errors, control chattering and insufficient operational stability, under low-speed, small-turning-radius and frequent start-stop conditions.

[0005] This invention provides a method for low-speed unmanned vehicle trajectory tracking and motion control, comprising the following steps:

[0006] Step 1: Obtain the reference path and perform path geometry modeling. Discretize the reference path into discrete points, label the curvature, and insert a zero-velocity self-calibration window to obtain the reference path table.

[0007] Step 2: Align the vehicle's initial heading based on the reference path table, load the constant table, and set the initial bias of the inertial measurement unit to zero to obtain the vehicle attitude and constant table.

[0008] Step 3: Perform nearest point matching and error calculation. Based on the vehicle attitude and reference path table, determine the path tangential direction of the discrete points of the path to obtain the lateral error, heading error and current path curvature.

[0009] Step 4: Perform nonholonomic constraint projection and curvature consistency constraint. Based on the vehicle attitude, obtain the longitudinal velocity and yaw rate to obtain the instantaneous curvature, and establish curvature consistency constraint with the current path curvature.

[0010] Step 5: Based on lateral error, heading error, current path curvature and instantaneous curvature, a steering request is generated under the static integrated solution of curvature to steering and the static back mapping of actuators, and the steering actuator input is obtained according to the maximum rate of change limit of steering input.

[0011] Step 6: Determine the upper limit of curvature constraint speed based on the constant table and the current path curvature; determine the upper limit of steering dynamic constraint speed based on the constant table and the steering actuator input; determine the reference speed based on the upper limit of curvature constraint speed and the upper limit of steering dynamic constraint speed; and combine the speed of the previous sampling period to obtain the current actual speed.

[0012] Step 7: Perform deterministic reset and abnormal degradation processing within the zero-speed self-calibration window to update the constant table.

[0013] Further, a reference path is obtained and its geometric model is performed. The reference path is discretized into discrete points, curvatures are labeled, and a zero-velocity self-calibration window is inserted to obtain a reference path table, including:

[0014] Step 11: Obtain reference path control points. Collect the coordinates of multiple reference path control points in the coordinate system according to the driving sequence, and connect the reference path control points to form a reference path.

[0015] Step 12: Perform path geometry modeling on the reference path, calculate the Euclidean distance between two adjacent reference path control points in sequence, accumulate the Euclidean distances in order to obtain the arc length index corresponding to each reference path control point, and perform interpolation fitting on the reference path based on the arc length index to establish a path geometry model with the arc length index as a parameter.

[0016] Step 13: Based on the path geometry model and the preset arc length step, generate an arc length index sequence within the arc length index range according to the preset arc length step. For each arc length index in the arc length index sequence, calculate the corresponding path discrete point coordinates through the path geometry model to form a path discrete point. For each path discrete point, combine the coordinates of the adjacent path discrete points to determine the circle passing through the three points, and take the reciprocal of the radius of the circle as the curvature of the path discrete point, i.e., the path curvature.

[0017] Step 14: Determine multiple zero-speed self-calibration window arc length index intervals based on the curvature of each path discrete point. Mark the position corresponding to the arc length index within any zero-speed self-calibration window arc length index interval as a zero-speed self-calibration window marker, and mark the position corresponding to the arc length index not within any zero-speed self-calibration window arc length index interval as a non-zero-speed self-calibration window marker. Construct a reference path table based on the arc length index, path discrete points, curvature, and zero-speed self-calibration window markers.

[0018] Furthermore, the vehicle's initial heading is aligned based on the reference path table, a constant table is loaded, and the initial bias of the inertial measurement unit is set to zero to obtain the vehicle attitude and constant table, including:

[0019] Step 21: Based on the coordinates of the two discrete points with the smallest arc length index in the reference path table, divide the difference between the ordinates of the two points by the difference between their abscissas to obtain the arctangent value of the heading angle, and use this heading angle as the initial heading of the vehicle.

[0020] Step 22: Load the constant table and write the actuator dead zone threshold, actuator proportional coefficient, maximum jerk, maximum rate of change of steering input, and zero-speed window dwell time into the constant table.

[0021] Step 23: While the vehicle is stationary, continuously collect the angular velocity of the inertial measurement unit and calculate the average value. Use the average value as the initial bias of the inertial measurement unit and set it to zero. Combine the vehicle's initial heading, the coordinates of the discrete points of the path starting point of the reference path table, and the bias of the inertial measurement unit to form the vehicle attitude.

[0022] Further, nearest point matching and error calculation are performed. Based on the vehicle attitude and reference path table, the path tangential direction of discrete points on the path is determined, resulting in lateral error, heading error, and current path curvature, including:

[0023] Step 31: Calculate the Euclidean distance between two points based on the vehicle's current position coordinates in the vehicle posture and the coordinates of each path discrete point in the reference path table, and take the path discrete point with the minimum Euclidean distance as the nearest path discrete point.

[0024] Step 32: Based on the coordinates of the nearest path discrete point and its adjacent path discrete points in the arc length index order, calculate the arctangent value by dividing the difference in the ordinate of the two points by the difference in the abscissa, determine the path tangential direction, and use the path tangential direction as the path direction for error calculation.

[0025] Step 33: Based on the vehicle's current position coordinates and heading angle in the vehicle's attitude, and based on the path tangential direction determined in Step 32, the vehicle's current position coordinates relative to the coordinates of the nearest path discrete point are projected onto the normal direction established by the path tangential direction to obtain the lateral error. The heading error is obtained by the difference between the vehicle's heading angle and the path tangential direction. The path curvature corresponding to the nearest path discrete point is read from the reference path table to obtain the lateral error, heading error, and current path curvature.

[0026] Furthermore, nonholonomic constraint projection and curvature consistency constraints are executed. Based on the vehicle attitude, the longitudinal velocity and yaw rate are obtained to obtain the instantaneous curvature, and curvature consistency constraints are established with the current path curvature, including:

[0027] Step 41: Based on the longitudinal velocity and lateral velocity in the vehicle posture, set the lateral velocity in the vehicle posture to zero according to the zero lateral velocity constraint of ground wheeled vehicles to obtain the vehicle posture that satisfies the nonholonomic constraint.

[0028] Step 42: Based on the longitudinal velocity and yaw rate in the vehicle attitude obtained in Step 41, the instantaneous curvature is obtained by the ratio of the yaw rate to the longitudinal velocity.

[0029] Step 43: Based on the instantaneous curvature and the current path curvature, calculate the difference between the instantaneous curvature and the current path curvature and use this difference as a curvature consistency constraint.

[0030] Furthermore, a steering request is generated under the static integrated solution of curvature to steering and the static inverse mapping of the actuator, and the steering actuator input is obtained according to the maximum rate of change limit of the steering input, including:

[0031] Step 51: Multiply the current path curvature by the vehicle wheelbase in the constant table and calculate the arctangent to obtain the static feedforward.

[0032] Step 52: Calculate the difference between the current roadbed curvature and the instantaneous curvature to obtain the curvature difference. Based on the lateral error, heading error and curvature difference, multiply them item by item with the lateral error feedback gain, heading error feedback gain and curvature difference feedback gain in the constant table, and sum the products to obtain the steering feedback amount.

[0033] Step 53: Summing the static feedforward quantity and the steering feedback quantity yields the steering request. Based on the steering request and the actuator dead zone threshold and actuator proportional coefficient in the constant table, a segmented static reverse mapping is performed to obtain the static mapping input. Based on the difference between the static mapping input and the steering actuator input of the previous sampling period, the difference is restricted to an allowable range determined by the maximum rate of change of the steering input to obtain the steering actuator input.

[0034] Furthermore, the upper limit of curvature constraint speed is determined based on the constant table and the current path curvature, and the upper limit of steering dynamic constraint speed is determined based on the constant table and the steering actuator input, including:

[0035] Step 61: Divide the maximum lateral acceleration in the constant table by the absolute value of the current path curvature, and take the square root of the resulting ratio to determine the upper limit of the curvature constraint velocity.

[0036] Step 62: Based on the steering actuator input, the steering angle is obtained by static reverse mapping through the actuator proportional coefficient and the actuator dead zone threshold; the tangent of the steering angle is divided by the vehicle wheelbase to obtain the equivalent curvature.

[0037] Step 63: Divide the maximum yaw rate in the constant table by the absolute value of the equivalent curvature to obtain the upper limit of the steering dynamic constraint speed.

[0038] Furthermore, a reference speed is determined based on the upper limit of the curvature constraint speed and the upper limit of the steering dynamic constraint speed, and the current actual speed is obtained by combining it with the speed of the previous sampling period, including:

[0039] Step 71: Take the smaller value between the upper limit of the curvature constraint speed and the upper limit of the steering dynamic constraint speed as the reference speed;

[0040] Step 72: Calculate the speed difference between the reference speed and the speed of the previous sampling period;

[0041] Step 73: Based on the maximum acceleration and sampling period in the constant table, limit the velocity difference to the allowable variation range defined by the product of the maximum acceleration and the sampling period, and add the limited velocity difference to the velocity of the previous sampling period to obtain the current actual velocity.

[0042] Furthermore, within the zero-speed self-calibration window, deterministic reset and anomalous degradation processing are performed to update the constant table, including:

[0043] Step 81: Based on the arc length index matching reference path table in the vehicle attitude, when the actual speed of the vehicle is lower than the zero speed determination threshold and the duration of the vehicle maintaining zero speed reaches the zero speed parking time, it is determined to enter the zero speed self-correction state and the reference speed is set to zero.

[0044] Step 82: During the zero-speed self-calibration state, a fixed number of inertial measurement unit angular velocity samples are collected and the average angular velocity is calculated. The average angular velocity is written into the constant table as the angular velocity offset. The yaw rate is corrected according to the angular velocity offset, and the vehicle heading angle is updated according to the tangential direction of the path corresponding to the current arc length index.

[0045] Step 83: During the zero-speed self-calibration state, read the steering angle measurement value and compare it with the theoretical zero steering angle to obtain the steering zero point deviation. Update the actuator dead zone threshold in the constant table according to the steering zero point deviation, and update the actuator proportional coefficient according to the ratio of the steering angle change measured before and after the update to the steering angle change before the update.

[0046] This invention also provides a low-speed unmanned vehicle trajectory tracking and motion control system, including:

[0047] The path modeling generation module is used to obtain reference paths and perform path geometry modeling. It discretizes the reference paths into path discrete points, labels the curvature, and inserts zero-speed self-calibration windows to obtain a reference path table.

[0048] The initialization module is used to align the vehicle's initial heading based on the reference path table, load the constant table, and set the initial bias of the inertial measurement unit to zero to obtain the vehicle attitude and constant table.

[0049] The error calculation and matching module is used to perform nearest point matching and error calculation. Based on the vehicle attitude and reference path table, it determines the path tangential direction of the discrete points of the path and obtains the lateral error, heading error and current path curvature.

[0050] The consistency constraint module is used to perform nonholonomic constraint projection and curvature consistency constraints. Based on the vehicle attitude, the longitudinal velocity and yaw rate are obtained to obtain the instantaneous curvature, and curvature consistency constraints are established with the current path curvature.

[0051] The steering solution control module is used to generate a steering request based on lateral error, heading error, current path curvature and instantaneous curvature, under the static integrated solution of curvature to steering and the static inverse mapping of actuators, and to obtain the steering actuator input according to the maximum rate of change limit of steering input;

[0052] The speed shaping generation module is used to determine the upper limit of curvature constraint speed based on the constant table and the current path curvature, determine the upper limit of steering dynamic constraint speed based on the constant table and the steering actuator input, determine the reference speed based on the upper limit of curvature constraint speed and the upper limit of steering dynamic constraint speed, and combine the speed of the previous sampling period to obtain the current actual speed.

[0053] The self-correcting degradation module is used to perform deterministic reset and abnormal degradation processing within the zero-speed self-correction window, and to update the constant table.

[0054] The beneficial effects of this invention are as follows: By constructing a reference path table within a unified arc length parameter space, this invention integrates path discrete points, path curvature, and zero-speed self-calibration windows, achieving consistent access to path geometric information throughout the trajectory tracking process; based on nonholonomic constraint projection and instantaneous curvature calculation, the vehicle can maintain kinematic constraint self-consistency under low-speed and near-zero-speed conditions, improving the stability of attitude estimation; in terms of steering control, a combination of static feedforward, error feedback, and actuator static back-mapping is adopted, explicitly incorporating actuator dead zone thresholds, proportional coefficients, and input rate of change limits, enabling steering control to match the true response characteristics of the actuator and suppressing chattering and overshoot in the low-speed stage; furthermore, by setting a zero-speed self-calibration window, this invention utilizes deterministic updates of inertial measurement unit bias, heading angle, and steering actuator parameters during vehicle stationary periods to achieve periodic self-calibration of sensor bias and actuator parameters, effectively reducing long-term error accumulation caused by drift and aging; overall, this invention can improve the trajectory tracking accuracy, control smoothness, and long-term operational reliability of low-speed unmanned vehicles. Attached Figure Description

[0055] Figure 1 This is a flowchart of the low-speed unmanned vehicle trajectory tracking and motion control method of the present invention. Detailed Implementation

[0056] The subject matter described herein will now be discussed with reference to exemplary embodiments. It should be understood that these embodiments are discussed only to enable those skilled in the art to better understand and implement the subject matter described herein, and changes may be made to the function and arrangement of the elements discussed without departing from the scope of this specification. Various processes or components may be omitted, substituted, or added as needed in the examples. Furthermore, features described in some examples may be combined in other examples.

[0057] like Figure 1 As shown, the low-speed unmanned vehicle trajectory tracking and motion control method includes:

[0058] Step 1: Obtain the reference path and perform path geometry modeling. Discretize the reference path into discrete points, label the curvature, and insert a zero-velocity self-calibration window to obtain the reference path table.

[0059] Step 2: Align the vehicle's initial heading based on the reference path table, load the constant table, and set the initial bias of the inertial measurement unit to zero to obtain the vehicle attitude and constant table.

[0060] Step 3: Perform nearest point matching and error calculation. Based on the vehicle attitude and reference path table, determine the path tangential direction of the discrete points of the path to obtain the lateral error, heading error and current path curvature.

[0061] Step 4: Perform nonholonomic constraint projection and curvature consistency constraint. Based on the vehicle attitude, obtain the longitudinal velocity and yaw rate to obtain the instantaneous curvature, and establish curvature consistency constraint with the current path curvature.

[0062] Step 5: Based on lateral error, heading error, current path curvature and instantaneous curvature, a steering request is generated under the static integrated solution of curvature to steering and the static back mapping of actuators, and the steering actuator input is obtained according to the maximum rate of change limit of steering input.

[0063] Step 6: Determine the upper limit of curvature constraint speed based on the constant table and the current path curvature; determine the upper limit of steering dynamic constraint speed based on the constant table and the steering actuator input; determine the reference speed based on the upper limit of curvature constraint speed and the upper limit of steering dynamic constraint speed; and combine the speed of the previous sampling period to obtain the current actual speed.

[0064] Step 7: Perform deterministic reset and abnormal degradation processing within the zero-speed self-calibration window to update the constant table.

[0065] In one embodiment of the present invention, a reference path is obtained and path geometry modeling is performed. The reference path is discretized into discrete points, curvature is labeled, and a zero-velocity self-calibration window is inserted to obtain a reference path table, including:

[0066] Step 11: Obtain reference path control points through vehicle sensors or map data; collect the coordinates of multiple reference path control points in the driving sequence in the coordinate system; and connect the reference path control points to form a reference path.

[0067] Step 12: Perform path geometry modeling on the reference path. Calculate the Euclidean distance between adjacent control points of the reference path sequentially, and sum the Euclidean distances in order to obtain the arc length index corresponding to each control point of the reference path, which is the cumulative distance from the path start point to the current control point. Then, perform interpolation fitting on the reference path based on the arc length index to establish a path geometry model with the arc length index as a parameter. This path geometry model can describe the shape of the path based on the arc length, so that the path coordinates corresponding to any arc length index can be determined by the path geometry model.

[0068] Step 13: Based on the path geometry model and the preset arc length step, generate an arc length index sequence within the arc length index range according to the preset arc length step. For each arc length index in the arc length index sequence, calculate the corresponding path discrete point coordinates through the path geometry model to form a path discrete point. For each path discrete point, determine the circle passing through the three points by combining the coordinates of the adjacent path discrete points. Use the reciprocal of the radius of the circle as the curvature of the path discrete point, i.e., the path curvature, so that the arc length index, the path discrete point and the path curvature are associated in a one-to-one correspondence.

[0069] Step 14: Determine multiple zero-speed self-calibration window arc length index intervals based on the curvature of each path discrete point. The zero-speed self-calibration window is used to perform state calibration of the vehicle under low-speed driving conditions to avoid error accumulation. Mark the position corresponding to the arc length index within any zero-speed self-calibration window arc length index interval as the zero-speed self-calibration window mark, and mark the position corresponding to the arc length index not within any zero-speed self-calibration window arc length index interval as the non-zero-speed self-calibration window mark. A reference path table is constructed based on the arc length index, path discrete points, curvature, and zero-speed self-calibration window marks.

[0070] Specifically, a curvature threshold is set. When the curvature of the path exceeds the threshold, the path is considered a curve, which may affect the vehicle's precision control. Therefore, a zero-speed self-correction window is introduced. For each discrete point on the path, if its curvature is greater than the curvature threshold, a zero-speed self-correction window is inserted around the arc length index of that point. Path discrete points within the arc length index range of the zero-speed self-correction window are marked as zero-speed self-correction window markers; conversely, path discrete points not within the zero-speed self-correction window are marked as non-zero-speed self-correction window markers. In this way, the reference path table contains the curvature and zero-speed self-correction window markers for each path discrete point, ensuring that the vehicle can improve control precision through the self-correction process within the window when driving at low speeds or stationary.

[0071] Through the above process, this embodiment can effectively discretize the path and accurately label the path curvature at low speeds. At the same time, through the design of the zero-speed self-calibration window, it ensures that the vehicle can perform accurate path tracking and motion control during low-speed driving. The zero-speed self-calibration window provides high-precision path information for the trajectory tracking control system by combining path geometric modeling and curvature labeling with real-time feedback of vehicle status, and avoids the error accumulation problem that may occur in traditional path tracking methods.

[0072] In one embodiment of the present invention, the vehicle's initial heading is aligned based on a reference path table, a constant table is loaded, and the initial bias of the inertial measurement unit is set to zero to obtain the vehicle attitude and constant table, including:

[0073] Step 21: Based on the coordinates of the two discrete points with the smallest arc length index in the reference path table, which are the starting position and the next path point respectively, the arctangent value of the heading angle is obtained by dividing the difference in the ordinates of the two points by the difference in their abscissas. This heading angle is used as the initial heading of the vehicle, i.e., the vehicle's facing direction. The formula for calculating the initial heading of the vehicle is: , Indicates the vehicle's initial heading. and These represent the coordinates of the starting position of the path and the coordinates of the next path point, respectively. Represents the arctangent function in the four quadrants;

[0074] Step 22: Load the constant table, writing the actuator dead zone threshold, actuator proportional coefficient, maximum jerk, maximum rate of change of steering input, and zero-speed window dwell time into the constant table; where, the actuator dead zone threshold represents the response area of ​​the steering actuator when the input signal is close to zero, used to suppress small control errors; the actuator proportional coefficient is used to determine the proportional relationship between the input steering command and the actual response of the steering actuator; the maximum jerk is used to limit the jerk of the vehicle to avoid stability problems caused by excessive speed changes; the maximum rate of change of steering input represents the maximum rate at which steering input changes, ensuring smooth steering response and avoiding control instability caused by abrupt steering; the zero-speed window dwell time represents the time the vehicle stays at zero speed, ensuring sufficient time for self-correction.

[0075] Step 23: While the vehicle is stationary, continuously collect the angular velocity of the inertial measurement unit (IMU) and calculate the average value. Use this average value as the initial bias of the IMU and set it to zero. Combine the vehicle's initial heading, the coordinates of the discrete points on the path starting point of the reference path table, and the IMU bias to form the vehicle attitude. The vehicle attitude and the constant table serve as the reference for the path tracking control system, ensuring that the vehicle maintains the correct driving direction and attitude at all times during trajectory tracking.

[0076] In one embodiment of the present invention, nearest point matching and error calculation are performed. Based on the vehicle attitude and reference path table, the path tangential direction of discrete points on the path is determined to obtain lateral error, heading error, and current path curvature, including:

[0077] Step 31: Calculate the Euclidean distance between two points based on the vehicle's current position coordinates in the vehicle posture and the coordinates of each path discrete point in the reference path table, and use the path discrete point with the minimum Euclidean distance as the nearest path discrete point as the reference point for the current vehicle.

[0078] Step 32: Based on the coordinates of the nearest path discrete point and its adjacent path discrete points in the arc length index order, calculate the arctangent value by dividing the difference in the ordinate of the two points by the difference in the abscissa, determine the path tangential direction, and use the path tangential direction as the path direction for error calculation.

[0079] Step 33: Based on the vehicle's current position coordinates and heading angle in the vehicle's attitude, and based on the path tangential direction determined in Step 32, the vehicle's current position coordinates relative to the coordinates of the nearest path discrete point are projected onto the normal direction established by the path tangential direction to obtain the lateral error. The heading error is obtained by the difference between the vehicle's heading angle and the path tangential direction. The path curvature corresponding to the nearest path discrete point is read from the reference path table to obtain the lateral error, heading error, and current path curvature.

[0080] The formula for calculating the lateral error is as follows:

[0081] , Indicates lateral error. Indicates the current position coordinates. This represents the coordinates of the discrete point on the path closest to the current position. The tangential direction of the path is represented by sin and cos, respectively, which are sine and cosine functions. By employing lateral error and heading error feedback control, the vehicle can accurately follow the predetermined path in low-speed or complex environments.

[0082] This embodiment performs nearest point matching and error calculation, combined with path geometric modeling, to accurately calculate lateral error, heading error and path curvature, ensuring that the low-speed unmanned vehicle can maintain high-precision path control during trajectory tracking.

[0083] In one embodiment of the present invention, nonholonomic constraint projection and curvature consistency constraint are performed. Based on the vehicle attitude, the longitudinal velocity and yaw rate are obtained to obtain the instantaneous curvature, and a curvature consistency constraint is established with the current path curvature, including:

[0084] Step 41: Based on the longitudinal velocity and lateral velocity in the vehicle attitude, the lateral velocity in the vehicle attitude is set to zero according to the zero lateral velocity constraint of ground wheeled vehicles, thus obtaining a vehicle attitude that satisfies the nonholonomic constraints. This step ensures that the vehicle's velocity state satisfies the nonholonomic constraints of an ideal vehicle without side slippers, providing a stable input for subsequent instantaneous curvature calculation.

[0085] Step 42: Based on the longitudinal velocity and yaw rate in the vehicle attitude obtained in Step 41, the instantaneous curvature is obtained by the ratio of the yaw rate to the longitudinal velocity; the instantaneous curvature is calculated to improve the vehicle's response to path geometry under low-speed conditions.

[0086] Step 43: Based on the instantaneous curvature and the current path curvature, calculate the difference between the instantaneous curvature and the current path curvature, and use this difference as a curvature consistency constraint. This step, through the curvature consistency constraint, can suppress curvature deviations caused by sensor noise or low-speed drift, thereby improving the steering control accuracy during low-speed unmanned vehicle path tracking.

[0087] This embodiment achieves the constraint of vehicle attitude quantities, real-time estimation of curvature quantities, and geometric consistency correction by performing nonholonomic constraint projection, instantaneous curvature calculation, and curvature consistency constraint establishment. Nonholonomic constraints improve the physical reliability of vehicle motion state, instantaneous curvature calculation enhances the instantaneous response of vehicle state to path curvature, and curvature difference establishes the constraint relationship between vehicle state and path geometry. This maintains higher stability and control accuracy in low-speed unmanned vehicle trajectory tracking, providing a reliable data foundation for the overall motion control system.

[0088] In one embodiment of the present invention, a steering request is generated based on lateral error, heading error, current path curvature, and instantaneous curvature, under the static integrated solution of curvature to steering and the static inverse mapping of the actuator. The steering actuator input is obtained according to the maximum rate of change limit of the steering input, including:

[0089] Step 51: Multiply the current path curvature by the vehicle wheelbase in the constant table and calculate the arctangent to obtain the static feedforward. The static feedforward is used to adjust the steering angle according to the curvature of the path to ensure that the vehicle can turn along the expected path and avoid trajectory errors caused by slow steering.

[0090] Step 52: Calculate the difference between the current roadbed curvature and the instantaneous curvature to obtain the curvature difference. Based on the lateral error, heading error, and curvature difference, multiply each item by the lateral error feedback gain, heading error feedback gain, and curvature difference feedback gain in the constant table, and sum the products to obtain the steering feedback amount. Among them, the lateral error feedback gain, heading error feedback gain, and curvature difference feedback gain are set fixed proportional coefficients used to adjust the degree of influence of different error terms on the steering feedback amount. The steering feedback amount is a correction amount generated based on the current error and gain value, used to correct the vehicle's steering angle and ensure that the vehicle can correct any path deviation.

[0091] Step 53: Summing the static feedforward quantity and the steering feedback quantity yields the steering request. Based on the steering request and the actuator dead zone threshold and actuator proportional coefficient in the constant table, a segmented static reverse mapping is performed to obtain the static mapping input. Based on the difference between the static mapping input and the steering actuator input of the previous sampling period, the difference is restricted to an allowable range determined by the maximum rate of change of the steering input to obtain the steering actuator input.

[0092] Specifically, when the absolute value of the steering request is less than or equal to the actuator dead zone threshold, the static mapping input is 0; when the steering request is greater than the actuator dead zone threshold, the static mapping input is calculated using the following formula: , Indicates statically mapped input, This represents the actuator proportional coefficient, with a value ranging from 0.5 to 5. This indicates a redirection request, and D represents the actuator dead zone threshold. The symbolic function outputs 1 when the steering request is greater than 0, 0 when the steering request is equal to 0, and -1 when the steering request is less than 0. This symbolic function is used to keep the direction of the actuator input consistent with the direction of the steering request. This step means that when the steering request is small, no steering action is performed, and when the steering request is large, the dead zone is crossed first, and then the speed is increased proportionally.

[0093] This embodiment introduces lateral error, heading error, and path curvature difference into the steering control strategy, combines static feedforward and feedback control, and static back-mapping and input variation constraints of the actuator, to provide a low-speed unmanned vehicle trajectory tracking control method. This method can accurately control the vehicle's steering in low-speed or complex environments, enabling it to stably and accurately follow the predetermined path, thereby improving the path tracking performance and control accuracy of low-speed unmanned vehicles.

[0094] In one embodiment of the present invention, determining the curvature constraint speed limit based on a constant table and the current path curvature, and determining the steering dynamic constraint speed limit based on a constant table and the steering actuator input, includes:

[0095] Step 61: Divide the maximum lateral acceleration in the constant table by the absolute value of the current path curvature, and take the square root of the resulting ratio to determine the upper limit of the curvature constraint speed. This step constrains the vehicle speed by the degree of path curvature and reduces the risk of lateral instability of the vehicle in sharp turns or narrow roads by using curvature constraints.

[0096] Step 62: Based on the steering actuator input, the steering angle is obtained by statically back-mapping the actuator proportional coefficient and the actuator dead zone threshold; the tangent of this steering angle is divided by the vehicle wheelbase to obtain the equivalent curvature; the formula for calculating the steering angle is: , Indicates the steering angle. This represents the steering actuator input; this step converts the actuator control quantity into a physically interpretable steering angle and then into an equivalent curvature by performing a static inverse mapping on the steering actuator input. This calculation quantifies the vehicle's currently achievable steering capability.

[0097] Step 63: Divide the maximum yaw rate in the constant table by the absolute value of the equivalent curvature to obtain the upper limit of the steering dynamic constraint speed. This upper limit of the steering dynamic constraint speed is used to limit the maximum permissible speed of the vehicle under a given steering capability, so as to avoid the situation where the vehicle cannot reach the desired turning radius at high speed.

[0098] This embodiment derives both curvature constraint speed limits and steering dynamic constraint speed limits based on path geometric constraints and actuator dynamic capabilities, achieving dual constraints on vehicle speed. By combining path geometric features with vehicle steering capabilities, this embodiment ensures stable operation of low-speed unmanned vehicles in conditions such as small-radius curves, complex environments, or actuator limitations, thereby improving the system's trajectory tracking accuracy and operational safety under weak dynamic conditions.

[0099] In one embodiment of the present invention, a reference speed is determined based on the upper limit of curvature constraint speed and the upper limit of steering dynamic constraint speed, and the current actual speed is obtained by combining the speed of the previous sampling period, including:

[0100] Step 71: Take the smaller value between the curvature constraint speed limit and the steering dynamic constraint speed limit as the reference speed; this step can automatically reduce the speed when the path curvature increases or the actuator steering capability is insufficient, thereby improving the vehicle's trajectory tracking stability.

[0101] Step 72: Calculate the speed difference between the reference speed and the speed of the previous sampling period;

[0102] Step 73: Based on the maximum acceleration and sampling period in the constant table, limit the speed difference within the allowable variation range defined by the product of the maximum acceleration and the sampling period. Then, add the limited speed difference to the speed of the previous sampling period to obtain the current actual speed. The product of the maximum acceleration and the sampling period forms the maximum allowable speed difference for the vehicle within one sampling period, and its range is as follows: , Indicates the maximum acceleration. Indicates the sampling period. This step represents the speed difference; by limiting the speed difference based on the maximum acceleration, the vehicle speed is updated smoothly, avoiding longitudinal instability caused by speed jumps; by introducing a speed limit range defined by the maximum acceleration and the sampling period, the vehicle can gradually approach the reference speed with physically achievable acceleration under all low-speed conditions, thereby ensuring the safety and comfort of the vehicle's longitudinal dynamics and improving the trajectory tracking and speed control performance of low-speed unmanned vehicles.

[0103] This embodiment achieves dynamic calculation of the vehicle's current actual speed through three steps: reference speed determination, speed difference calculation, and speed shaping. This enables low-speed unmanned vehicles to maintain stable operation in complex paths and environments with limited steering capabilities, thereby enhancing the vehicle's trajectory tracking ability and longitudinal control safety.

[0104] In one embodiment of the present invention, deterministic reset and anomalous degradation processing are performed within a zero-rate self-calibration window to update the constant table, including:

[0105] Step 81: Based on the arc length index matching reference path table in the vehicle attitude, when the actual speed of the vehicle is lower than the zero speed determination threshold and the duration of the vehicle maintaining zero speed reaches the zero speed parking time, it is determined to enter the zero speed self-correction state and the reference speed is set to zero to avoid unnecessary longitudinal speed disturbances.

[0106] Step 82: During the zero-speed self-calibration state, a fixed number of inertial measurement unit angular velocity samples are collected and the average angular velocity is calculated. The average angular velocity is written into the constant table as the angular velocity offset. The yaw rate is corrected according to the angular velocity offset, that is, the yaw rate is subtracted from the angular velocity offset to obtain the corrected yaw rate. The vehicle heading angle is updated according to the path tangential direction corresponding to the current arc length index, that is, the path tangential direction corresponding to the current arc length index is used as the vehicle heading angle, so that the heading is realigned with the path direction.

[0107] Step 83: During the zero-speed self-calibration state, the steering angle measurement value is read and compared with the theoretical zero steering angle to obtain the steering zero-point deviation. The theoretical zero steering angle is 0 in the zero-speed self-calibration state, and the steering zero-point deviation represents the actual deviation of the actuator when it perceives itself to be 0 degrees. The actuator dead-zone threshold in the constant table is updated based on the steering zero-point deviation. Specifically, the set dead-zone adjustment coefficient is multiplied by the absolute value of the steering zero-point deviation, and the updated actuator dead-zone threshold is added to obtain the updated actuator dead-zone threshold. This updated threshold is used to compensate for zero-point drift caused by factors such as mechanical backlash and friction in the steering mechanism. The actuator proportional coefficient is updated based on the ratio of the change in steering angle measured before and after the update to the change in steering angle before the update. This step, by simultaneously updating the steering actuator dead-zone threshold and proportional coefficient, can automatically calibrate the actuator zero point and gain in a stable environment where the vehicle is stationary, eliminating the effects of actuator hysteresis, offset, and output degradation.

[0108] This embodiment achieves an automatic recovery and compensation mechanism for the vehicle attitude calculation module and the actuator control module by performing angular velocity offset calibration, heading alignment, steering actuator zero-point calibration and gain update within the zero-speed self-calibration window. This can eliminate inertial drift and actuator degradation when the vehicle is stationary and safe, and improve the stability and reliability of low-speed unmanned vehicles in long-term operation, frequent start-stop and high repetitive working conditions.

[0109] This invention provides a low-speed unmanned vehicle trajectory tracking and motion control system, comprising:

[0110] The path modeling generation module is used to obtain reference paths and perform path geometry modeling. It discretizes the reference paths into path discrete points, labels the curvature, and inserts zero-speed self-calibration windows to obtain a reference path table.

[0111] The initialization module is used to align the vehicle's initial heading based on the reference path table, load the constant table, and set the initial bias of the inertial measurement unit to zero to obtain the vehicle attitude and constant table.

[0112] The error calculation and matching module is used to perform nearest point matching and error calculation. Based on the vehicle attitude and reference path table, it determines the path tangential direction of the discrete points of the path and obtains the lateral error, heading error and current path curvature.

[0113] The consistency constraint module is used to perform nonholonomic constraint projection and curvature consistency constraints. Based on the vehicle attitude, the longitudinal velocity and yaw rate are obtained to obtain the instantaneous curvature, and curvature consistency constraints are established with the current path curvature.

[0114] The steering solution control module is used to generate a steering request based on lateral error, heading error, current path curvature and instantaneous curvature, under the static integrated solution of curvature to steering and the static inverse mapping of actuators, and to obtain the steering actuator input according to the maximum rate of change limit of steering input;

[0115] The speed shaping generation module is used to determine the upper limit of curvature constraint speed based on the constant table and the current path curvature, determine the upper limit of steering dynamic constraint speed based on the constant table and the steering actuator input, determine the reference speed based on the upper limit of curvature constraint speed and the upper limit of steering dynamic constraint speed, and combine the speed of the previous sampling period to obtain the current actual speed.

[0116] The self-correcting degradation module is used to perform deterministic reset and abnormal degradation processing within the zero-speed self-correction window, and to update the constant table.

[0117] It should be noted that the interval and threshold sizes are set for ease of comparison. The size of the threshold depends on the amount of sample data and the base number set by those skilled in the art for each set of sample data, as long as it does not affect the proportional relationship between the parameter and the quantized value. Furthermore, the above formulas are all dimensionless calculations, and the formulas are derived from software simulations using a large amount of collected data to obtain the most recent real-world results. The preset parameters in the formulas are set by those skilled in the art according to the actual situation.

[0118] The embodiments of the present invention have been described above, but the present invention is not limited to the specific embodiments described above. The specific embodiments described above are merely illustrative and not restrictive. Those skilled in the art can make many other forms based on the guidance of the present embodiments, all of which are within the protection scope of the present embodiments.

Claims

1. A method for trajectory tracking and motion control of low-speed unmanned vehicles, characterized in that, Includes the following steps: Step 1: Obtain the reference path and perform path geometry modeling. Discretize the reference path into discrete points, label the curvature, and insert a zero-velocity self-calibration window to obtain the reference path table. Step 2: Align the vehicle's initial heading based on the reference path table, load the constant table, and set the initial bias of the inertial measurement unit to zero to obtain the vehicle attitude and constant table. Step 3: Perform nearest point matching and error calculation. Based on the vehicle attitude and reference path table, determine the path tangential direction of the discrete points of the path to obtain the lateral error, heading error and current path curvature. Step 4: Perform nonholonomic constraint projection and curvature consistency constraint. Based on the vehicle attitude, obtain the longitudinal velocity and yaw rate to obtain the instantaneous curvature, and establish curvature consistency constraint with the current path curvature. Step 5: Based on lateral error, heading error, current path curvature and instantaneous curvature, a steering request is generated under the static back mapping of the actuator through the static integrated solution of curvature to steering, and the steering actuator input is obtained according to the maximum rate of change limit of steering input. Step 6: Determine the upper limit of curvature constraint speed based on the constant table and the current path curvature; determine the upper limit of steering dynamic constraint speed based on the constant table and the steering actuator input; determine the reference speed based on the upper limit of curvature constraint speed and the upper limit of steering dynamic constraint speed; and combine the speed of the previous sampling period to obtain the current actual speed. Step 7: Perform deterministic reset and anomalous degradation processing within the zero-speed self-calibration window to update the constant table, including: Step 81: Based on the arc length index matching reference path table in the vehicle attitude, when the actual speed of the vehicle is lower than the zero speed determination threshold and the duration of the vehicle maintaining zero speed reaches the zero speed parking time, it is determined to enter the zero speed self-correction state and the reference speed is set to zero. Step 82: During the zero-speed self-calibration state, a fixed number of inertial measurement unit angular velocity samples are collected and the average angular velocity is calculated. The average angular velocity is written into the constant table as the angular velocity offset. The yaw rate is corrected according to the angular velocity offset, and the vehicle heading angle is updated according to the tangential direction of the path corresponding to the current arc length index. Step 83: During the zero-speed self-calibration state, read the steering angle measurement value and compare it with the theoretical zero steering angle to obtain the steering zero point deviation. Update the actuator dead zone threshold in the constant table according to the steering zero point deviation, and update the actuator proportional coefficient according to the ratio of the steering angle change measured before and after the update to the steering angle change before the update.

2. The low-speed unmanned vehicle trajectory tracking and motion control method according to claim 1, characterized in that, Obtain a reference path and perform path geometry modeling. Discretize the reference path into discrete points, label the curvature, and insert a zero-velocity self-calibration window to obtain a reference path table, including: Step 11: Obtain reference path control points. Collect the coordinates of multiple reference path control points in the coordinate system according to the driving sequence, and connect the reference path control points to form a reference path. Step 12: Perform path geometry modeling on the reference path, calculate the Euclidean distance between two adjacent reference path control points in sequence, accumulate the Euclidean distances in order to obtain the arc length index corresponding to each reference path control point, and perform interpolation fitting on the reference path based on the arc length index to establish a path geometry model with the arc length index as a parameter. Step 13: Based on the path geometry model and the preset arc length step, generate an arc length index sequence within the arc length index range according to the preset arc length step. For each arc length index in the arc length index sequence, calculate the corresponding path discrete point coordinates through the path geometry model to form a path discrete point. For each path discrete point, combine the coordinates of the adjacent path discrete points to determine the circle passing through the three points, and take the reciprocal of the radius of the circle as the curvature of the path discrete point, i.e., the path curvature. Step 14: Determine multiple zero-speed self-calibration window arc length index intervals based on the curvature of each path discrete point. Mark the position corresponding to the arc length index within any zero-speed self-calibration window arc length index interval as a zero-speed self-calibration window marker, and mark the position corresponding to the arc length index not within any zero-speed self-calibration window arc length index interval as a non-zero-speed self-calibration window marker. Construct a reference path table based on the arc length index, path discrete points, curvature, and zero-speed self-calibration window markers.

3. The low-speed unmanned vehicle trajectory tracking and motion control method according to claim 1, characterized in that, The vehicle's initial heading is aligned, a constant table is loaded, and the initial bias of the inertial measurement unit is set to zero to obtain the vehicle attitude and constant table, including: Step 21: Based on the coordinates of the two discrete points with the smallest arc length index in the reference path table, divide the difference between the ordinates of the two points by the difference between their abscissas to obtain the arctangent value of the heading angle, and use this heading angle as the initial heading of the vehicle. Step 22: Load the constant table and write the actuator dead zone threshold, actuator proportional coefficient, maximum jerk, maximum rate of change of steering input, and zero-speed window dwell time into the constant table. Step 23: While the vehicle is stationary, continuously collect the angular velocity of the inertial measurement unit and calculate the average value. Use the average value as the initial bias of the inertial measurement unit and set it to zero. Combine the vehicle's initial heading, the coordinates of the discrete points of the path starting point of the reference path table, and the bias of the inertial measurement unit to form the vehicle attitude.

4. The low-speed unmanned vehicle trajectory tracking and motion control method according to claim 1, characterized in that, Perform nearest point matching and error calculation to determine the path tangential direction of discrete points on the path, and obtain the lateral error, heading error, and current path curvature, including: Step 31: Calculate the Euclidean distance between two points based on the vehicle's current position coordinates in the vehicle posture and the coordinates of each path discrete point in the reference path table, and take the path discrete point with the minimum Euclidean distance as the nearest path discrete point. Step 32: Based on the coordinates of the nearest path discrete point and its adjacent path discrete points in the arc length index order, calculate the arctangent value by dividing the difference in the ordinate of the two points by the difference in the abscissa, determine the path tangential direction, and use the path tangential direction as the path direction for error calculation. Step 33: Based on the vehicle's current position coordinates and heading angle in the vehicle's attitude, and based on the path tangential direction determined in Step 32, the vehicle's current position coordinates relative to the coordinates of the nearest path discrete point are projected onto the normal direction established by the path tangential direction to obtain the lateral error. The heading error is obtained by the difference between the vehicle's heading angle and the path tangential direction. The path curvature corresponding to the nearest path discrete point is read from the reference path table to obtain the lateral error, heading error, and current path curvature.

5. The low-speed unmanned vehicle trajectory tracking and motion control method according to claim 1, characterized in that, Perform nonholonomic constraint projection and curvature consistency constraints to determine instantaneous curvature, and establish curvature consistency constraints with the current path curvature, including: Step 41: Based on the longitudinal velocity and lateral velocity in the vehicle posture, set the lateral velocity in the vehicle posture to zero according to the zero lateral velocity constraint of ground wheeled vehicles to obtain the vehicle posture that satisfies the nonholonomic constraint. Step 42: Based on the longitudinal velocity and yaw rate in the vehicle attitude obtained in Step 41, the instantaneous curvature is obtained by the ratio of the yaw rate to the longitudinal velocity. Step 43: Based on the instantaneous curvature and the current path curvature, calculate the difference between the instantaneous curvature and the current path curvature and use this difference as a curvature consistency constraint.

6. The low-speed unmanned vehicle trajectory tracking and motion control method according to claim 1, characterized in that, Through a static integrated solution from curvature to steering, a steering request is generated under static inverse mapping of the actuator, and the steering actuator input is obtained based on the maximum rate of change constraint of the steering input, including: Step 51: Multiply the current path curvature by the vehicle wheelbase in the constant table and calculate the arctangent to obtain the static feedforward. Step 52: Calculate the difference between the current roadbed curvature and the instantaneous curvature to obtain the curvature difference. Based on the lateral error, heading error and curvature difference, multiply them item by item with the lateral error feedback gain, heading error feedback gain and curvature difference feedback gain in the constant table, and sum the products to obtain the steering feedback amount. Step 53: Summing the static feedforward quantity and the steering feedback quantity yields the steering request. Based on the steering request and the actuator dead zone threshold and actuator proportional coefficient in the constant table, a segmented static reverse mapping is performed to obtain the static mapping input. Based on the difference between the static mapping input and the steering actuator input of the previous sampling period, the difference is restricted to an allowable range determined by the maximum rate of change of the steering input to obtain the steering actuator input.

7. The low-speed unmanned vehicle trajectory tracking and motion control method according to claim 1, characterized in that, Determine the upper limit of the curvature constraint speed and the upper limit of the steering dynamic constraint speed, including: Step 61: Divide the maximum lateral acceleration in the constant table by the absolute value of the current path curvature, and take the square root of the resulting ratio to determine the upper limit of the curvature constraint velocity. Step 62: Based on the steering actuator input, the steering angle is obtained by static reverse mapping through the actuator proportional coefficient and the actuator dead zone threshold; the tangent of the steering angle is divided by the vehicle wheelbase to obtain the equivalent curvature. Step 63: Divide the maximum yaw rate in the constant table by the absolute value of the equivalent curvature to obtain the upper limit of the steering dynamic constraint speed.

8. The low-speed unmanned vehicle trajectory tracking and motion control method according to claim 1, characterized in that, The process of determining the reference speed and the current actual speed includes: Step 71: Take the smaller value between the upper limit of the curvature constraint speed and the upper limit of the steering dynamic constraint speed as the reference speed; Step 72: Calculate the speed difference between the reference speed and the speed of the previous sampling period; Step 73: Based on the maximum acceleration and sampling period in the constant table, limit the velocity difference to the allowable variation range defined by the product of the maximum acceleration and the sampling period, and add the limited velocity difference to the velocity of the previous sampling period to obtain the current actual velocity.

9. A low-speed unmanned vehicle trajectory tracking and motion control system, characterized in that, The low-speed unmanned vehicle trajectory tracking and motion control method as described in any one of claims 1-8 includes: The path modeling generation module is used to obtain reference paths and perform path geometry modeling. It discretizes the reference paths into path discrete points, labels the curvature, and inserts zero-speed self-calibration windows to obtain a reference path table. The initialization module is used to align the vehicle's initial heading based on the reference path table, load the constant table, and set the initial bias of the inertial measurement unit to zero to obtain the vehicle attitude and constant table. The error calculation and matching module is used to perform nearest point matching and error calculation. Based on the vehicle attitude and reference path table, it determines the path tangential direction of the discrete points of the path and obtains the lateral error, heading error and current path curvature. The consistency constraint module is used to perform nonholonomic constraint projection and curvature consistency constraints. Based on the vehicle attitude, the longitudinal velocity and yaw rate are obtained to obtain the instantaneous curvature, and curvature consistency constraints are established with the current path curvature. The steering solution control module is used to generate a steering request based on the lateral error, heading error, current path curvature and instantaneous curvature through static integrated solution of curvature to steering, and obtain the steering actuator input according to the maximum rate of change limit of steering input. The speed shaping generation module is used to determine the upper limit of curvature constraint speed based on the constant table and the current path curvature, determine the upper limit of steering dynamic constraint speed based on the constant table and the steering actuator input, determine the reference speed based on the upper limit of curvature constraint speed and the upper limit of steering dynamic constraint speed, and combine the speed of the previous sampling period to obtain the current actual speed. The self-correcting degradation module is used to perform deterministic reset and abnormal degradation processing within the zero-speed self-correction window, and to update the constant table.

Images