A virtual control point-based slope trajectory tracking control method and system for unmanned tracked vehicles

Abstract

This invention discloses a method and system for tracking and controlling the trajectory of an unmanned tracked vehicle on a slope based on virtual control points, belonging to the field of tracked vehicle trajectory tracking and control. First, the gravitational yaw moment caused by the vehicle's center of gravity offset is calculated, and a discrete prediction model of slope dynamics is constructed. Second, virtual control points are generated, and the influence of lateral slippage is corrected through an anti-slip compensation term. Finally, the tracked vehicle's attitude information is acquired in real time, and based on the discrete prediction model of slope dynamics, the predicted value of the state vector is obtained. The predicted coordinates of the virtual control points are calculated, and the optimal control quantity is solved. This invention innovatively proposes a dynamic migration method for "virtual control points" based on center of gravity offset and slope parameters. This method migrates the control reference point from the geometric center to a virtual position that can balance the gravitational moment, effectively solving the influence of yaw moment on the tracked vehicle's control attitude, suppressing the slippage influence caused by the lateral component of gravity, and effectively solving the instability of tracked vehicles in slope motion control.

Description

Technical Field

[0001] This invention belongs to the field of tracked vehicle trajectory tracking control, and relates to a method and system for unmanned tracked vehicle slope trajectory tracking control based on virtual control points. Background Technology

[0002] Unmanned tracked vehicles play a crucial role in unstructured environment tasks such as mountain exploration, autonomous agricultural machinery operation in hilly areas, disaster search and rescue, and military maneuvers due to their low ground pressure, strong obstacle-crossing ability, and high adaptability to harsh road surfaces. Trajectory tracking, as the core underlying task of autonomous driving, directly affects the continuity and safety of operations. Most existing motion control schemes, such as those based on model predictive control (MPC) and linear quadratic regulators (LQR), typically assume that the geometric center (GC) of the vehicle chassis is the control force point and that it coincides with the vehicle's physical center of mass (CoG) in the horizontal projection plane. However, in actual field operations, vehicles frequently face steep cross slopes, longitudinal slopes, and complex composite slopes. In these situations, the component of gravity along the slope generates enormous lateral thrust and longitudinal disturbances, causing the interaction between the tracks and the soil to exhibit highly nonlinear mechanical characteristics, with a generally non-negligible spatial displacement between the center of mass and the geometric center. Because of the lever arm between the center of mass and the geometric center, the gravitational component generates a yaw moment about the geometric center. Even if the front of the vehicle (geometric center) barely stays on the line, the rear of the vehicle will still experience a huge sideslip due to gravity. Since traditional feedback control is a passive logic of "correcting after generating errors," the vehicle must physically maintain a constant lateral displacement deviation to counteract the gravitational component. This results in an unavoidable static residual in trajectory tracking, making true path closure impossible. For example, Chinese invention patent CN117565870A discloses an ultra-low speed predictive control method for off-road unmanned vehicles on slopes. It collects point cloud data of the road ahead, corrects it, extracts longitudinal profile information, calculates the longitudinal slope angle change law, and optimizes the driving force and braking force output through model predictive control strategy. However, it only predicts and controls the longitudinal slope, does not consider the impact of the lateral slope on the vehicle, and does not involve steering control logic. The center of gravity shift caused by the lateral slope will affect the tracking accuracy and attitude control of the tracked vehicle on the slope. Chinese invention patent CN118651244A discloses a motion control method for unmanned tracked vehicles on multi-directional slope sections. It constructs a prediction model, calculates the equivalent slope angle and direction angle by fitting the plane normal vector through point cloud, determines the vehicle motion state equation to predict the pre-arrival position, and realizes vehicle motion control. However, it does not consider the dynamic characteristics of load changes and center of gravity shifts on the slope, which affects the trajectory accuracy and attitude stability of the tracked vehicle on the slope.

[0003] It is evident that most existing motion control schemes fail to consider the center of gravity shift of tracked vehicles and the control situation on slopes. Furthermore, they all assume the control point is the geometric center of the tracked vehicle, leading to reduced trajectory accuracy, unstable attitude control, and even instability due to gravity causing sliding downhill. Therefore, there is an urgent need for a method that can actively sense the physical center of gravity shift and dynamically adjust the control strategy to solve the instability problems caused by slippage and center of gravity shift in traditional schemes for slope control. Summary of the Invention

[0004] The purpose of this invention is to provide a method and system for tracking and controlling the slope trajectory of unmanned tracked vehicles based on virtual control points.

[0005] This invention is achieved through the following technical solution:

[0006] The first aspect of this invention provides a method for tracking and controlling the trajectory of an unmanned tracked vehicle on a slope based on virtual control points, comprising:

[0007] S1. Based on the real-time analysis of the slope gravity components, calculate the gravity yaw moment caused by the vehicle's center of gravity offset, construct the slope dynamics prediction model and discretize it to obtain the slope dynamics discrete prediction model.

[0008] S2, generate virtual control points, determine the coordinates of the virtual control points based on the vehicle's geometric center and centroid position and dynamic weight coefficients, and then correct the lateral slippage effect through anti-slip compensation terms;

[0009] S3 defines the objective function and constraints for integrating trajectory deviation, energy consumption and ride comfort; acquires tracked vehicle attitude information in real time, obtains state vector prediction values ​​based on the slope dynamics discrete prediction model, calculates the predicted coordinates of virtual control points, and uses a quadratic programming algorithm to solve for the optimal control quantity.

[0010] Furthermore, S1 specifically refers to:

[0011] Construct a vehicle coordinate system and a geodetic coordinate system. Calculate the three gravity components, including longitudinal drag, in the vehicle coordinate system. Lateral lateral slip force and vertical inclined plane downward pressure Calculate the gravitational yaw moment according to the torque formula. .

[0012] Define state vector ,in This represents the coordinates of the vehicle's geometric center in the geodetic coordinate system. For heading angle, The longitudinal linear velocity, Define the yaw rate; define the control input. , , These are the driving forces for the left and right tracks, respectively.

[0013] Establish a slope dynamics prediction model:

[0014] (1);

[0015] Where m is the mass of the tracked vehicle, g is the acceleration due to gravity, G is the vehicle weight, L is the track contact length, B is the track center distance, and h is the height of the vehicle's center of gravity. The rolling resistance coefficient, This is the steering resistance coefficient when turning on a slope. Let the vehicle's yaw moment of inertia be... and These are the rolling resistances of the left and right tracks, respectively. For steering resistance torque, , , , They are respectively The derivative of The slope angle, The coordinates of the vehicle's center of mass in the vehicle body coordinate system.

[0016] The forward Euler method is used to discretize the slope dynamics prediction model, and the sampling time is set to be... Tracked vehicles in The state at time is The state at the next moment The derivation is as follows:

[0017] (2);

[0018] Expanding equation (2) according to equation (1), we obtain the discrete prediction model for slope dynamics:

[0019] (3);

[0020] In the formula, ,in These represent the coordinates of the vehicle's geometric center in the geodetic coordinate system at time k, along with its heading angle, longitudinal linear velocity, and yaw rate; the control input vector. , These represent the driving forces of the left and right tracks at time k, respectively; and These represent the rolling resistances of the left and right tracks at time k, respectively. The turning resistance torque at time k; The gravitational yaw moment at time k; Let k be the slope angle at time k.

[0021] Furthermore, S2 specifically refers to:

[0022] S201, Constructing Virtual Control Points The tracking error is calculated based on this virtual control point to mitigate the effects of yaw moment and lateral slip. The coordinates of the virtual control point at time k are... The calculation formula is as follows:

[0023] (4);

[0024] in: The real-time position coordinates of the vehicle's geometric center in the geodetic coordinate system; This is the coordinate rotation matrix for transforming the vehicle coordinate system to the geodetic coordinate system; This is a dynamic weighting coefficient, with a value range of [value range missing]. , This represents the offset vector of the vehicle's center of mass relative to its geometric center in the vehicle coordinate system. This is an anti-slip compensation term.

[0025] S202, calculated according to the following formula :

[0026] (5);

[0027] in, , The deviation between the vehicle's geometric center and center of mass in the geodetic coordinate system; for The lateral speed of the vehicle at any given moment; This is the gain coefficient.

[0028] S203, construct the following nonlinear mapping relationship and calculate the dynamic weight coefficients:

[0029] (6);

[0030] in, The gravitational yaw moment at time k is calculated in real time; This represents the upper limit of the maximum steering resistance torque that a tracked vehicle can provide under the current terrain conditions, and has... ; This is the sensitivity adjustment factor, determined through offline calibration.

[0031] Furthermore, S3 specifically refers to:

[0032] Construct the following objective function The aim is to minimize virtual control points. Compared with reference trajectory The deviation between them, while constraining the smoothness of the input quantity:

[0033] (7);

[0034] (8);

[0035] (9);

[0036] (10);

[0037] in, The objective function for tracking the target trajectory; The objective function is to control the magnitude of the driving force; The objective function is to control the rate of change of the driving force; This indicates the prediction time domain, including time after the current time k. One time step; This indicates the control time domain, including time after the current time k. One time step; These are the weight matrices for the corresponding terms; Prediction at time k The predicted position coordinates of the virtual control point at any given time in the geodetic coordinate system. This represents the reference trajectory point position coordinates corresponding to the predicted position coordinates of the virtual control point. This indicates the prediction at time k. The amount of control input at any given time.

[0038] Set the following constraints:

[0039] (11);

[0040] in, Predict the driving force for the left and right tracks. and These are the minimum and maximum values ​​of the track driving force, respectively. , These are the predicted values ​​for longitudinal linear velocity and yaw rate, respectively. , These are the limits for longitudinal linear velocity and yaw rate, respectively.

[0041] Real-time acquisition of the tracked vehicle's actual state parameters at the current moment, constructing the actual state vector at the current moment. The future can be predicted using a discrete prediction model of slope dynamics. State vector prediction within prediction steps And then calculate the future Predicted position coordinates of virtual control points within the prediction steps Based on the future The predicted position coordinates of the virtual control points are determined based on the slope dynamics discrete prediction model, with the objective function minimized. Based on the constraints to be satisfied, the optimal control quantity sequence is obtained by solving the problem using a quadratic programming algorithm. The first control input in the sequence The data is then sent to the tracked vehicle for control. In the next instant, the actual state parameters of the tracked vehicle are collected again in real time, and the processes of state vector prediction, virtual control point coordinate calculation, and solving using a quadratic programming algorithm are repeated. The control is then issued to the tracked vehicles for vehicle control, and this process is repeated.

[0042] Another aspect of the present invention provides a slope trajectory tracking control system for unmanned tracked vehicles based on virtual control points, comprising:

[0043] The upper planning layer module is used to plan the tracked vehicle trajectory and output the reference state vector and reference control input;

[0044] The model building module is used for real-time analysis based on the slope gravity components, dynamically deriving the gravity yaw moment caused by the centroid offset, constructing a slope dynamics prediction model and discretizing it to obtain a discrete slope dynamics prediction model.

[0045] The virtual control point generation module is used to generate virtual control points. It determines the coordinates of the virtual control points based on the vehicle's geometric center and center of gravity positions and dynamic weighting coefficients, and then corrects the lateral slippage effect through anti-slip compensation terms.

[0046] The vehicle control module is used to define the objective function and constraints. Based on the real-time acquired tracked vehicle attitude information, it predicts the state vector value using a discrete prediction model of slope dynamics. The virtual control point prediction coordinates are calculated by the virtual control point generation module, and the optimal control quantity is solved using a quadratic programming algorithm.

[0047] Compared with the prior art, the present invention has at least the following beneficial effects:

[0048] 1. This invention abandons the traditional geometric center control strategy and innovatively proposes a "virtual control point" dynamic migration method based on centroid offset and slope parameters. According to the current slope angle, heading angle and vehicle centroid position, the control reference point is migrated from the geometric center to a virtual position that can balance the gravitational torque in real time, effectively solving the influence of yaw torque on the tracked vehicle's control attitude and suppressing the slippage effect caused by the lateral component of gravity.

[0049] 2. This invention establishes an MPC dynamic prediction model that includes the yaw moment of slope gravity, enabling MPC to predict system states that are more consistent with real slope scenarios. Attached Figure Description

[0050] Figure 1 Here is a flowchart of the overall process for the slope trajectory tracking and control method of unmanned tracked vehicles based on virtual control points;

[0051] Figure 2 Force diagram of the center of mass shift during the slope motion of the tracked vehicle;

[0052] Figure 3 Virtual control points for tracked vehicle's slope movement Generate flowchart;

[0053] Figure 4 The diagram shows the lateral sliding motion generated by the tracked vehicle as it travels along the right front of the ramp. Detailed Implementation

[0054] To further understand the implementation method of the present invention, the technical solutions in the embodiments of the present invention are clearly and completely described. However, the embodiments described below are merely a part of the present invention, and not all of it. The following description is only for further illustrating the advantages and features of the present invention, and not for claiming limitations on the present invention. Other embodiments obtained by those skilled in the art without inventive effort are all within the scope of protection of the present invention.

[0055] The purpose of this invention is to provide a method and system for tracking and controlling the trajectory of unmanned tracked vehicles on slopes based on virtual control points, which involves a multi-stage collaborative process centered around vehicle control.

[0056] The present invention will be further described below with reference to the accompanying drawings and specific embodiments.

[0057] The first aspect of this embodiment provides a slope trajectory tracking and control method for unmanned tracked vehicles based on virtual control points, the process of which is as follows: Figure 1 As shown, it includes:

[0058] S1. Based on the real-time analysis of the gravity components of the slope, the gravity yaw moment caused by the centroid offset is dynamically derived, a slope dynamic prediction model is constructed and discretized to obtain the slope dynamic prediction model.

[0059] S101, Establish a coordinate system;

[0060] Establish a geodetic coordinate system with any point on the slope as the origin, defining the positive half-axis of the Y-axis as upward along the slope and the positive half-axis of the X-axis as to the right along the slope; establish a vehicle coordinate system with the geometric center (GC) of the tracked vehicle as the origin, with the x-axis pointing towards the front of the vehicle, the y-axis pointing towards the left side of the vehicle, and the z-axis pointing upward perpendicular to the slope.

[0061] Let the vehicle's center of gravity be... The coordinates in the vehicle coordinate system are: ,when or When , it indicates the existence of a centroid offset.

[0062] S102, Real-time analysis of the gravity components of the slope;

[0063] like Figure 2 As shown, when a tracked vehicle travels on a slope, gravity is no longer perpendicular to the ground, but is decomposed into a component along the vehicle's axis. The pitch angle of the tracked vehicle is collected in real time by an onboard inertial measurement unit (IMU). (i.e., the slope angle) and heading angle Let the mass of the tracked vehicle be m, and the acceleration due to gravity be g. In the vehicle's coordinate system, the three gravitational components caused by the slope are calculated as follows:

[0064] ① The longitudinal component of gravity along the x-axis, i.e., the longitudinal drag: ;

[0065] ② The lateral component of gravity along the y-axis, i.e., the lateral sliding force: ;

[0066] ③ The vertical pressure of gravity along the z-axis, i.e., the pressure perpendicular to the inclined plane: .

[0067] S103, Modeling of gravitational yaw moment caused by center of mass offset;

[0068] In existing technologies, it is typically assumed that the point of application of gravity coincides with the geometric center, neglecting the effect of gravity on steering. However, in the actual slope movement conditions of tracked vehicles, due to the vehicle's center of gravity... It does not coincide with the geometric center, that is, there exists an offset vector. Then the gravitational component will generate an additional gravitational yaw moment on the inclined plane. .

[0069] Define gravitational yaw moment The counterclockwise direction is positive, and the clockwise direction is negative. According to the torque formula, the gravitational yaw torque... The calculation formula is as follows:

[0070] ;

[0071] .

[0072] S104, Construct a continuous-time slope dynamics prediction model;

[0073] Define state vector ,in This represents the coordinates of the vehicle's geometric center in the geodetic coordinate system. For heading angle, The longitudinal linear velocity, The yaw rate is obtained in real time by the onboard inertial navigation system; the control input is defined. , , These are the driving forces for the left and right tracks, respectively.

[0074] Based on the Newton-Euler equations and the three gravitational components, a slope dynamics prediction model is established:

[0075] (1);

[0076] Where G is the vehicle weight, L is the track contact length, B is the track center distance, and h is the vehicle's center of gravity height. The rolling resistance coefficient, This is the steering resistance coefficient when turning on a slope. Let the vehicle's yaw moment of inertia be... and These are the rolling resistances of the left and right tracks, respectively. For steering resistance torque, , , , They are respectively The derivative of .

[0077] S105, Discretization of the slope dynamics prediction model;

[0078] To facilitate implementation in a digital controller (MPC solver), the continuous differential equation shown in equation (1) is discretized using the forward Euler method. Let the sampling time be... Tracked vehicles in The state at time is The state at the next moment The derivation is as follows:

[0079] (2);

[0080] Expanding equation (2) according to equation (1), we obtain the discrete prediction model for slope dynamics:

[0081] (3);

[0082] In the formula, ,in These represent the coordinates of the vehicle's geometric center in the geodetic coordinate system at time k, along with its heading angle, longitudinal linear velocity, and yaw rate; the control input vector. , These represent the driving forces of the left and right tracks at time k, respectively; and These represent the rolling resistances of the left and right tracks at time k, respectively. The turning resistance torque at time k; The gravitational yaw moment at time k; Let k be the slope angle at time k.

[0083] S2, generate virtual control points, determine the coordinates of the virtual control points based on the vehicle's geometric center and centroid position and dynamic weight coefficients, and then correct the lateral slippage effect through anti-slip compensation terms;

[0084] S201, Construct a virtual control point mapping model;

[0085] When a tracked vehicle is performing trajectory tracking motion control on a slope, with the vehicle's geometric center as the control point, the gravitational yaw moment will affect the tracked vehicle's attitude due to the shift in the center of gravity, resulting in lateral sliding force. Slippage will occur under the influence of the terrain slope and the vehicle's slippage state. Therefore, a virtual control point is constructed that dynamically changes with the terrain slope and the vehicle's slippage state, denoted as... The tracking error is calculated based on this virtual control point, thereby reducing the impact of yaw moment and lateral slip.

[0086] Let the coordinates of the vehicle's geometric center in the vehicle body coordinate system be... The centroid coordinates are After constructing a virtual control point coordinate mapping model and rotating it to the geodetic coordinate system, the coordinates of the virtual control point at time k are obtained. The calculation formula is as follows, and the calculation process is as follows: Figure 3 As shown:

[0087] (4);

[0088] in: The real-time position of the vehicle's geometric center in the geodetic coordinate system is obtained by the onboard inertial navigation system. This is the coordinate rotation matrix for transforming the vehicle coordinate system to the geodetic coordinate system; This is a dynamic weighting coefficient, with a value range of [value range missing]. This is used to adjust the system's attention weights to geometric time features and dynamic features. This represents the offset vector of the vehicle's center of mass relative to its geometric center in the vehicle coordinate system. This is an anti-slip compensation term used to counteract lateral slippage caused by the lateral component of gravity.

[0089] S202, Obtain anti-slip compensation terms ;

[0090] To accurately capture instantaneous skidding, the vehicle's lateral velocity in the vehicle's coordinate system is obtained using the onboard inertial navigation system and then converted to the geodetic coordinate system for calculation. This is used to generate additional control force (i.e., driving force) for tracked vehicles to control their steering, and to suppress lateral slippage caused by the lateral component of gravity by changing the heading angle. Specifically:

[0091] The actual lateral slip speed generated by the tracked vehicle Equivalent to the vehicle's lateral speed output in real time by the in-vehicle inertial navigation system. Then we have:

[0092] ;

[0093] in, It is a vector composed of the lateral and longitudinal sliding velocities in the vehicle body coordinate system. The longitudinal sliding velocity, Let be the lateral sliding velocity; in the above formula, let A value of 0 indicates that the focus is on the unstable effects of lateral slip velocity on trajectory tracking control.

[0094] Calculate according to the following formula :

[0095] (5);

[0096] in, , The deviation between the vehicle's geometric center and center of mass in the geodetic coordinate system; for The vehicle's lateral velocity output by the in-vehicle inertial navigation system at all times (positive for left and negative for right). This is the gain coefficient.

[0097] Actual lateral sliding force The impact analysis is as follows:

[0098] like Figure 4 As shown, assuming the tracked vehicle travels along the reference trajectory upwards and to the right of the slope, the tracked vehicle is subjected to a gravitational component acting downwards along the slope. Less than 0, heading angle Between 0 and 90 degrees Greater than 0, as can be seen from formula (5) = Less than 0, and similarly we can know = If the value is greater than 0, according to formula (4), the virtual control point moves a certain distance to the lower right relative to the reference trajectory. Since the vehicle calculates the tracking error based on the virtual control point, the tracked vehicle generates a lateral distance error to the right of the reference trajectory. Therefore, the MPC controller will generate a control quantity (driving force) to drive the tracked vehicle to turn slightly to the left to eliminate the error. At this time, the heading angle is... It will increase. It will decrease, according to the lateral sliding force formula. , The lateral slip force is thus reduced, thereby suppressing the lateral slippage of the tracked vehicle. Similarly, it can be analyzed that when the tracked vehicle travels in other directions, Actual lateral sliding force The effect of this is to suppress lateral slippage.

[0099] S203, Identify and determine dynamic weighting coefficients;

[0100] Utilizing the saturation property of the hyperbolic tangent function, the following nonlinear mapping relationship is constructed:

[0101] (6);

[0102] in, For real-time calculation of gravitational yaw moment; This represents the upper limit of the maximum steering resistance torque that a tracked vehicle can provide under the current terrain conditions, and has... ; This is the sensitivity adjustment factor, a dimensionless constant, used to adjust... The value represents the response speed to gravitational disturbances.

[0103] because It can be calculated in real time. Since it is a known quantity, it is necessary to consider a single parameter. Offline calibration is performed to give the tracked vehicle adaptive capabilities under different inclines. The offline calibration logic aims to find a balance point that can effectively utilize the vehicle's center of gravity to resist gravitational descent and yaw moment, without excessively sacrificing the trajectory fit to the vehicle's geometric center. The offline calibration process includes:

[0104] A1. Construct a benchmark test scenario and select a series of representative test slopes. , exemplary For each test slope, a standard semi-circular tracking path is planned, and the tracked vehicle is set to travel at the same target operating speed.

[0105] A2, Settings search space ; at a certain test slope Next, select within the search space. The values ​​were used to conduct a high-fidelity semi-circular trajectory simulation experiment. During the experiment, the dynamic weight coefficients were calculated in real time according to formula (6) and then substituted into formula (4) to calculate the coordinates of the virtual control points. And perform trajectory tracking control;

[0106] Construct the following discretized comprehensive evaluation function:

[0107] ;

[0108] in, This represents the vehicle sideslip angle at discrete time k; (k) represents the lateral tracking error at discrete time k; and These are the corresponding weighting coefficients; Indicates the sampling period. For a single test cycle The total number of steps within is then: .

[0109] Utilizing vehicle state parameters acquired by the onboard inertial navigation system, real-time calculations are performed. as well as (k), calculated according to the above formula value corresponding ; with the set step size, in Traversing within the search space Values, obtain each value corresponding For example, the step size is 0.25; select such that smallest The value is used as the test slope. Optimal sensitivity factor .

[0110] At each test slope Perform operation A2 separately to obtain the slope values ​​for each test. The optimal sensitivity factor under the given conditions forms a discrete mapping set. .

[0111] A3, in order to enable the tracked vehicle to cope with the test slope Scope, i.e. For slopes at any angle within the range, based on the discrete mapping set obtained from A2, a cubic spline interpolation algorithm is used to fit each discrete point to generate a function curve. A continuous slope-sensitivity mapping model was constructed. This ensures continuity at each node and smooth first derivatives.

[0112] During the movement of the tracked vehicle, the slope angle is acquired in real time at the control cycle frequency. The optimal sensitivity factor under the current working condition is obtained by searching through the slope-sensitivity mapping model. And substitute it into formula (6) to update the dynamic weight coefficient.

[0113] S3 defines the objective function and constraints for integrating trajectory deviation, energy consumption and ride comfort; real-time acquisition of tracked vehicle attitude information; based on the slope dynamics discrete prediction model, prediction of state vector values; calculation of predicted coordinates of virtual control points; and use of quadratic programming algorithm to solve for optimal control quantity.

[0114] For the discrete prediction model of slope dynamics, the following objective function is constructed. The aim is to minimize virtual control points. Compared with reference trajectory The deviation between them, while constraining the smoothness of the input quantity:

[0115] (7);

[0116] (8);

[0117] (9);

[0118] (10);

[0119] in, The objective function for tracking the target trajectory is used to predict and minimize the deviation between the virtual control point and the reference trajectory in the time domain, thereby achieving the trajectory tracking objective. The objective function is used to control the magnitude of the driving force, in order to avoid excessive driving force and optimize energy consumption; The objective function for controlling the rate of change of driving force is used to suppress the oscillation of control commands and make control smoother and more stable. This indicates the prediction time domain, including time after the current time k. One time step; This indicates the control time domain, including time after the current time k. One time step; These are the weight matrices for the corresponding terms; Prediction at time k The predicted position coordinates of the virtual control point at any given time in the geodetic coordinate system. This represents the coordinates of the reference trajectory point corresponding to the predicted position of the virtual control point. This indicates the prediction at time k. The amount of control input at any given time.

[0120] Set the following constraints:

[0121] (11);

[0122] in, Predict the driving force for the left and right tracks. and These are the minimum and maximum values ​​of the track driving force, respectively. , These are the predicted values ​​for longitudinal linear velocity and yaw rate, respectively. , These are the limits for longitudinal linear velocity and yaw rate, respectively.

[0123] The system collects real-time data on the tracked vehicle's actual state parameters, including the vehicle's geometric center coordinates in the geodetic coordinate system, heading angle, longitudinal linear velocity, yaw rate, slope angle, and lateral slip velocity, and constructs the current state vector. and control input The future can be predicted using a discrete prediction model of slope dynamics. State vector prediction within prediction steps Based on the future Predicted state vector values ​​within a prediction step, assuming the future... Predicting slope angle within steps and lateral speed Keeping it unchanged, calculate the future using formula (4). Predicted position coordinates of virtual control points within the prediction steps Based on the future The location coordinates of the virtual control points are predicted based on the slope dynamics discrete prediction model to minimize the objective function. Based on the constraints to be satisfied, rolling optimization control is performed using a quadratic programming algorithm to obtain the optimal control quantity sequence. Based on the principle of rolling optimization control, the first control variable in the sequence is... The data is sent to the tracked vehicle's underlying motor drive for vehicle control. In the next instant, the actual state parameters of the tracked vehicle are collected again in real time, and the processes of state vector prediction, virtual control point coordinate calculation, and solving using a quadratic programming algorithm are repeated. The control is then issued to the tracked vehicles for vehicle control, and this process is repeated.

[0124] Experiments were conducted using the dSPACE hardware-in-the-loop simulation platform in a slope scenario to verify the method provided in this embodiment. Experimental results show that the method enables the tracked vehicle to achieve good trajectory tracking, and the vehicle's posture remains stable during travel, without any instability such as sideslip or sliding down the slope, thus verifying the effectiveness and reliability of the method under slope conditions.

[0125] Another aspect of this embodiment provides an unmanned tracked vehicle ramp trajectory tracking control system based on virtual control points, including:

[0126] The upper planning layer module is used to plan the tracked vehicle trajectory and output the reference state vector and reference control input;

[0127] The model building module is used for real-time analysis based on the slope gravity components, dynamically deriving the gravity yaw moment caused by the centroid offset, constructing a slope dynamics prediction model and discretizing it to obtain a discrete slope dynamics prediction model.

[0128] The virtual control point generation module is used to generate virtual control points. It determines the coordinates of the virtual control points based on the vehicle's geometric center and center of gravity positions and dynamic weighting coefficients, and then corrects the lateral slippage effect through anti-slip compensation terms.

[0129] The vehicle control module is used to define the objective function and constraints. Based on the real-time acquired tracked vehicle attitude information, it predicts the state vector value using a discrete prediction model of slope dynamics. The virtual control point prediction coordinates are calculated by the virtual control point generation module, and the optimal control quantity is solved using a quadratic programming algorithm.

[0130] The embodiments described above are merely illustrative of implementation methods of the present invention and should not be construed as limiting the scope of the present invention. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of the present invention, and these modifications and improvements all fall within the protection scope of the present invention.

Claims

1. A method for tracking and controlling the trajectory of an unmanned tracked vehicle on a slope based on virtual control points, characterized in that, include: S1. Based on the real-time analysis of the slope gravity components, calculate the gravity yaw moment caused by the vehicle's center of gravity offset, construct the slope dynamics prediction model and discretize it to obtain the slope dynamics discrete prediction model. S2, generate virtual control points, determine the coordinates of the virtual control points based on the vehicle's geometric center and centroid position and dynamic weight coefficients, and then correct the lateral slippage effect through anti-slip compensation terms; S3 defines the objective function and constraints for integrating trajectory deviation, energy consumption and ride comfort; acquires tracked vehicle attitude information in real time, obtains state vector prediction values ​​based on the slope dynamics discrete prediction model, calculates the predicted coordinates of virtual control points, and uses a quadratic programming algorithm to solve for the optimal control quantity.

2. The method according to claim 1, characterized in that, Specifically, S1 refers to: Construct a vehicle coordinate system and a geodetic coordinate system. Calculate the three gravity components, including longitudinal drag, in the vehicle coordinate system. Lateral lateral slip force and vertical inclined plane downward pressure Calculate the gravitational yaw moment according to the torque formula. ; Define state vector ,in This represents the coordinates of the vehicle's geometric center in the geodetic coordinate system. For heading angle, The longitudinal linear velocity, Define the yaw rate; define the control input. , , These are the driving forces for the left and right tracks, respectively; Establish a slope dynamics prediction model: （1）； Where m is the mass of the tracked vehicle, g is the acceleration due to gravity, G is the vehicle weight, L is the track contact length, B is the track center distance, and h is the height of the vehicle's center of gravity. The rolling resistance coefficient, This is the steering resistance coefficient when turning on a slope. Let the vehicle's yaw moment of inertia be... and These are the rolling resistances of the left and right tracks, respectively. For steering resistance torque, , , , They are respectively The derivative of The slope angle, The coordinates of the vehicle's center of mass in the vehicle body coordinate system; The forward Euler method is used to discretize the slope dynamics prediction model, and the sampling time is set to be... Tracked vehicles in The state at time is The state at the next moment The derivation is as follows: （2）； Expanding equation (2) according to equation (1), we obtain the discrete prediction model for slope dynamics: （3）； In the formula, ,in These represent the coordinates of the vehicle's geometric center in the geodetic coordinate system at time k, along with its heading angle, longitudinal linear velocity, and yaw rate; the control input vector. , These represent the driving forces of the left and right tracks at time k, respectively; and These represent the rolling resistances of the left and right tracks at time k, respectively. The turning resistance torque at time k; The gravitational yaw moment at time k; Let k be the slope angle at time k.

3. The method according to claim 2, characterized in that, In the vehicle body coordinate system, the three gravitational components caused by the slope are calculated as follows: ① The longitudinal component of gravity along the x-axis, i.e., the longitudinal drag: ; ② The lateral component of gravity along the y-axis, i.e., the lateral sliding force: ; ③ The vertical pressure of gravity along the z-axis, i.e., the pressure perpendicular to the inclined plane: .

4. The method according to claim 2, characterized in that, According to the torque formula, the gravitational yaw torque The calculation formula is as follows: 。 5. The method according to claim 1, characterized in that, Specifically, S2 is: S201, Constructing Virtual Control Points The coordinates of the virtual control point at time k The calculation formula is as follows: （4）； in: The real-time position coordinates of the vehicle's geometric center in the geodetic coordinate system; This is the coordinate rotation matrix for transforming the vehicle coordinate system to the geodetic coordinate system; This is a dynamic weighting coefficient, with a value range of [value range missing]. , This represents the offset vector of the vehicle's center of mass relative to its geometric center in the vehicle coordinate system. This is an anti-slip compensation term; Slope angle at time k Let k be the heading angle; The coordinates of the vehicle's center of mass in the vehicle body coordinate system; S202, calculated according to the following formula : （5）； in, , The deviation between the vehicle's geometric center and center of mass in the geodetic coordinate system; for The lateral speed of the vehicle at any given moment; This is the gain coefficient; S203, construct the following nonlinear mapping relationship and calculate the dynamic weight coefficients: （6）； Where h is the height of the vehicle's center of gravity. The gravitational yaw moment at time k is calculated in real time; This represents the upper limit of the maximum steering resistance torque that a tracked vehicle can provide under the current terrain conditions, and has... ; This is the sensitivity adjustment factor, determined through offline calibration.

6. The method according to claim 5, characterized in that, The aforementioned sensitivity adjustment factor The offline calibration process includes: A1. Construct a benchmark test scenario and select a series of representative test slopes. For each test slope, a standard semi-circular tracking path is planned, and the tracked vehicle is set to travel at the same target operating speed. A2, Settings The search space; at a certain test slope Next, select within the search space. The values ​​were used to conduct a high-fidelity semi-circular trajectory simulation experiment. During the experiment, the dynamic weight coefficients were calculated in real time according to formula (6) and then substituted into formula (4) to calculate the coordinates of the virtual control points. And perform trajectory tracking control; Construct the following discretized comprehensive evaluation function: ； in, This represents the vehicle sideslip angle at discrete time k; (k) represents the lateral tracking error at discrete time k; and These are the corresponding weighting coefficients; Indicates the sampling period. For a single test cycle Total number of steps within; Calculate in real time using the acquired vehicle state parameters as well as (k), calculated according to the above formula value corresponding ; with the set step size, in Traversing within the search space Values, obtain each value corresponding ; Selecting makes smallest The value is used as the test slope. Optimal sensitivity factor ; At each test slope Perform operation A2 separately to obtain the slope values ​​for each test. The optimal sensitivity factor under the given conditions forms a discrete mapping set. ; A3, based on the discrete mapping set obtained from A2, uses a cubic spline interpolation algorithm to fit each discrete point, generating a function curve. A continuous slope-sensitivity mapping model was constructed. During the movement of the tracked vehicle, the slope angle is acquired in real time at a control cycle frequency. The optimal sensitivity factor under the current working condition is obtained by searching through the slope-sensitivity mapping model. And substitute it into formula (6) to update the dynamic weight coefficient.

7. The method according to claim 1, 2, or 5, characterized in that, Specifically, S3 refers to: Construct the following objective function The aim is to minimize virtual control points. Compared with reference trajectory The deviation between them, while constraining the smoothness of the input quantity: （7）； （8）； （9）； （10）； in, The objective function for tracking the target trajectory; The objective function is to control the magnitude of the driving force; The objective function is to control the rate of change of the driving force; This indicates the prediction time domain, including time after the current time k. One time step; This indicates the control time domain, including time after the current time k. One time step; These are the weight matrices for the corresponding terms; Prediction at time k The predicted position coordinates of the virtual control point at any given time in the geodetic coordinate system. This represents the reference trajectory point position coordinates corresponding to the predicted position coordinates of the virtual control point. This indicates the prediction at time k. Control input at any time; Set the following constraints: （11）； in, Predict the driving force for the left and right tracks. and These are the minimum and maximum values ​​of the track driving force, respectively. , These are the predicted values ​​for longitudinal linear velocity and yaw rate, respectively. , These are the limits for longitudinal linear velocity and yaw rate, respectively. Real-time acquisition of the tracked vehicle's actual state parameters at the current moment, constructing the actual state vector at the current moment. The future can be predicted using a discrete prediction model of slope dynamics. State vector prediction within prediction steps And then calculate the future Predicted position coordinates of virtual control points within the prediction steps Based on the future The predicted position coordinates of the virtual control points are determined based on the slope dynamics discrete prediction model, with the objective function minimized. Based on the constraints to be satisfied, the optimal control quantity sequence is obtained by solving the problem using a quadratic programming algorithm. The first control input in the sequence The data is sent to the tracked vehicle for vehicle control; at the next moment, the actual state parameters of the tracked vehicle are collected again in real time, and the state vector prediction, virtual control point coordinate calculation, and solution based on the quadratic programming algorithm are repeated. The control is then issued to the tracked vehicles for vehicle control, and this process is repeated.

8. A slope trajectory tracking control system for unmanned tracked vehicles based on virtual control points, used to implement the method described in any one of claims 1-7, characterized in that, include: The upper planning layer module is used to plan the tracked vehicle trajectory and output the reference state vector and reference control input; The model building module is used for real-time analysis based on the slope gravity components, dynamically deriving the gravity yaw moment caused by the centroid offset, constructing a slope dynamics prediction model and discretizing it to obtain a discrete slope dynamics prediction model. The virtual control point generation module is used to generate virtual control points. It determines the coordinates of the virtual control points based on the vehicle's geometric center and center of gravity positions and dynamic weighting coefficients, and then corrects the lateral slippage effect through anti-slip compensation terms. The vehicle control module is used to define the objective function and constraints. Based on the real-time acquired tracked vehicle attitude information, it predicts the state vector value using a discrete prediction model of slope dynamics. The virtual control point prediction coordinates are calculated by the virtual control point generation module, and the optimal control quantity is solved using a quadratic programming algorithm.

Images