Unmanned tracked vehicle ramp motion control method based on adaptive model prediction

Abstract

The invention is suitable for the technical field of unmanned driving control, and provides an unmanned tracked vehicle ramp motion control method based on adaptive model prediction, and the method comprises the following steps: sensing and identifying an equivalent slope angle and a course angle of a ramp where an unmanned tracked vehicle is located in real time; on the basis of a Kalman filtering algorithm, estimating a longitudinal resistance coefficient and a steering resistance coefficient of the current ground on line; based on the slip rate lookup table, acquiring slip rates in current and future prediction time domains, and obtaining a slip speed correction item; constructing a self-adaptive model predictive control framework according to the equivalent slope angle, the course angle, the longitudinal resistance coefficient, the steering resistance coefficient and the slip rate; and based on the adaptive model predictive control framework, solving an optimal control instruction through rolling optimization, and inputting the optimal control instruction to a vehicle execution mechanism. According to the method, the problems of model mismatch and precision degradation in the trajectory tracking process are effectively solved through adaptive model predictive control according to the motion control requirement of the unmanned tracked vehicle under the complex unstructured terrain.

Description

Technical Field

[0001] This invention belongs to the field of unmanned driving control technology, and in particular relates to a method for controlling the motion of unmanned tracked vehicles on slopes based on adaptive model prediction. Background Technology

[0002] Unmanned tracked vehicles, with their superior terrain adaptability, are increasingly widely used in environmental detection, disaster relief, military mobility, and agricultural automation. The accuracy of their trajectory tracking control directly determines operational efficiency and safety. In practical applications, unmanned tracked vehicles frequently operate in complex geographical environments (such as soft slopes, sandy areas, and wilderness). These unstructured terrains typically feature numerous undulating slopes, and the mechanical properties of the road surface medium (such as soft soil, gravel, and slippery surfaces) exhibit high nonlinearity and variability. This makes tracked vehicles highly susceptible to significant slippage and sideslip during operation, while the interaction between the tracks and the ground is extremely complex, posing a severe challenge to high-precision trajectory tracking control.

[0003] Currently, most tracked vehicle trajectory tracking technologies, both domestically and internationally, are developed based on structured hard surfaces or conventional flat off-road scenarios, making it difficult to adapt to the unique characteristics of complex terrain environments. Commonly used fixed control parameters in existing technologies (such as ground drag coefficient and adhesion coefficient) differ significantly from complex and variable actual working conditions, leading to excessive trajectory tracking deviations when applied directly. With the increasing demand for testing and application of unmanned tracked vehicles in complex terrains (such as variable slopes, significant slippage surfaces, and time-varying mechanical conditions), there is an urgent need for a high-precision trajectory tracking method adaptable to unstructured complex terrains to ensure the control reliability of vehicles in extreme environments such as slope conditions, slippage disturbances, and time-varying parameters.

[0004] Currently, the most common methods for trajectory tracking of unmanned tracked vehicles are fixed-parameter model predictive control (MPC) and linear quadratic regulator (LQR) methods. Fixed-parameter MPC methods typically preset dynamic parameters such as ground drag coefficient and steering drag coefficient, predict future states by constructing a vehicle kinematic / dynamic model, and solve for the optimal control sequence based on constraints. LQR methods, on the other hand, linearize the vehicle model and construct a quadratic objective function to solve for the optimal control law. Both methods calculate the deviation between the actual trajectory and the reference trajectory in real time and adjust the track driving force or steering angle to compensate for the deviation.

[0005] In trajectory tracking control in complex geographical environments (especially involving slopes and soft surfaces), the aforementioned fixed-parameter-based MPC and LQR methods have significant shortcomings and are difficult to meet the requirements of high-precision tracking control.

[0006] The fixed-parameter MPC method suffers from insufficient parameter adaptability. This method heavily relies on pre-set fixed ground parameters (longitudinal resistance parameter f, steering resistance parameter μ). However, in complex geographical environments, these parameters exhibit significant dynamic time-varying characteristics depending on the density, moisture content, and slope angle of the road surface medium. The fixed-parameter model is severely mismatched with actual working conditions, leading to a significant decrease in prediction accuracy. Furthermore, it lacks slip disturbance compensation, failing to construct a dynamic compensation mechanism for the significant slip effects in unstructured terrain. Slippage caused by insufficient track-ground grip leads to deviations between actual and theoretically predicted speeds, and the traditional MPC model does not incorporate a slip speed correction term, further exacerbating trajectory tracking errors.

[0007] LQR-based methods suffer from several drawbacks. First, linearization introduces modeling errors. In complex environments, vehicle dynamics exhibit strong nonlinearity (e.g., track-ground interaction torque, slope coupling effect). LQR's linearization process introduces non-negligible modeling errors, leading to a significant decrease in control accuracy under extreme conditions. Second, it suffers from poor adaptability to various operating conditions. Its control performance is highly dependent on the selection of the state weight matrix. However, in complex scenarios with constantly changing operating conditions (gradient slope, dynamic changes in slip ratio), a fixed weight matrix cannot adapt to all conditions, easily resulting in insufficient accuracy on smooth roads or poor stability on steep slopes. Third, it exhibits weak robustness. It is poorly resistant to slip disturbances and time-varying environmental parameters, lacking a dynamic correction mechanism. This causes deviations to accumulate with the travel distance, ultimately resulting in a significant deviation from the preset path.

[0008] For example, in the prior art: Patent CN114510063A discloses an unmanned tracked vehicle and its trajectory tracking control method and system; it constructs an objective function for the trajectory tracking controller based on a kinematic model, and sets constraints in combination with a dynamic model to form an MPC control model; then, it generates a reference trajectory based on the vehicle pose, road curvature, and real-time operating conditions, inputs this trajectory and vehicle tracking state information into the MPC model to obtain parameter combinations, and then outputs control parameters through an MLP neural network to finally complete trajectory tracking. This scheme achieves a balance between trajectory tracking accuracy, driving stability, and computational efficiency by integrating road curvature features and neural network optimization, thus improving the vehicle's adaptability to different driving conditions. Patent CN114355882A discloses a design method for a tracked unmanned vehicle trajectory tracking controller based on a dynamic model; it constructs a state-space equation based on the tracked vehicle's dynamic model, transforms the preset trajectory tracking objective function into a standard quadratic form; within each control cycle, it calculates the current optimal control sequence through rolling optimization of the objective function, and continuously iterates and updates the control commands to achieve trajectory tracking. This solution combines dynamic models with rolling optimization to ensure high tracking accuracy and driving stability for vehicles under various speed conditions. Patent CN117565870A discloses a predictive control method for ultra-low speeds on slopes for off-road unmanned vehicles. It collects point cloud data of the road ahead using LiDAR, corrects the data using an inertial navigation system, and extracts longitudinal profile information of the vehicle's direction of travel. Based on this information, it calculates the variation law of the longitudinal slope angle of the road ahead (slope angle function), and then, combined with the vehicle's current state, optimizes the output of driving force and braking force through a model predictive control strategy. This solution focuses on ultra-low speed scenarios, utilizing terrain prediction to achieve power adaptation on slope sections, thus improving the vehicle's driving stability on gentle slopes. Patent CN118651244A discloses a motion control method for unmanned tracked vehicles on multi-directional slope sections. First, a geodetic coordinate system is established, and the vehicle's reference trajectory, position information, and raw point cloud data from lidar are fused to obtain complete point cloud information in the geodetic coordinate system. Then, a prediction model is constructed, and the equivalent slope angle and direction angle are calculated by fitting the plane normal vectors of the point cloud to determine the vehicle's motion state equation to predict the pre-arrival position. Combining constraints and an objective function, the model predictive controller is invoked to solve for the optimal driving / braking force control sequence, and finally, the first term of the sequence is executed to achieve vehicle motion control. This scheme improves the stability of vehicles on complex slopes through multi-directional slope prediction and MPC optimization.

[0009] However, the trajectory tracking control scheme disclosed in patent CN114510063A only focuses on the road curvature characteristics and does not take into account the impact of road slope on vehicle dynamics, nor does it address the changes in ground parameters and motion slippage under complex terrain. In slope scenarios, these deficiencies lead to a severe mismatch between the model and actual working conditions: slope changes alter the vehicle's stress state; ground parameters of unstructured soft surfaces (such as longitudinal drag coefficient and steering drag coefficient) exhibit significant time-varying characteristics, making it impossible for fixed-parameter MPC models to adapt; and the slippage effect generated by the tracks further amplifies the trajectory tracking deviation, making it difficult to meet the requirements of high-precision control. The method disclosed in patent CN114355882A relies on a dynamic model that does not incorporate the impact of slope conditions on vehicle stress and motion characteristics, and the model parameters and constraints are all set based on flat road surfaces. In complex working environments, this design leads to control inaccuracies. The additional resistance and dynamic changes such as center of gravity shift caused by slopes increase the deviation between the prediction results of the fixed model and the actual state. Simultaneously, the time-varying ground parameters and track slippage effects of soft road surfaces further weaken the model's adaptability, making it difficult to meet the high-precision tracking requirements in complex terrain. Patent CN117565870A discloses an ultra-low speed prediction and control method for unmanned vehicles on slopes, but it only predicts and controls the longitudinal slope, neglecting the impact of lateral slopes and not addressing steering control logic. In complex geographical environments, these deficiencies lead to insufficient vehicle stability. Problems such as center of gravity shift and uneven track stress caused by lateral slopes expose the vehicle to the risk of rollover. Furthermore, the time-varying ground parameters and track slippage effects of unstructured road surfaces, combined with the complex terrain of coupled lateral and longitudinal slopes, greatly amplify trajectory deviations. Because the original solution lacks discussion of the steering control dimension, it is difficult to achieve comprehensive trajectory tracking. The motion control scheme disclosed in patent CN118651244A does not optimize for the physical characteristics of terrains such as loose sand and soft soil. It assumes fixed values ​​for ground parameters (longitudinal drag coefficient f, steering drag coefficient μ), ignoring the dynamic changes of these parameters with road surface compaction, moisture, and media properties in actual operating environments. Furthermore, the scheme fails to consider the significant track slippage effect on complex road surfaces, and the motion state equation does not incorporate a slippage velocity correction term, resulting in a large deviation between model predictions and actual driving conditions, and insufficient trajectory tracking accuracy. In addition, its MPC objective function only focuses on position deviation and signal smoothness, without incorporating a slip ratio control term.

[0010] In summary, while existing motion control schemes for unmanned tracked vehicles have achieved some success, significant limitations remain in complex terrain scenarios. Most schemes focus only on flat terrain, neglecting the impact of slopes on dynamics and trajectory tracking control. Furthermore, they generally fail to consider the time-varying characteristics of ground parameters in soft sand and soil conditions, and lack compensation mechanisms to address the track slippage effect that is prone to occur in these environments. These deficiencies lead to a mismatch between the model and actual operating conditions, amplified trajectory tracking deviations, and potentially even safety risks such as forced stopping or rollovers in complex environments with slopes. Summary of the Invention

[0011] The purpose of this invention is to provide a slope motion control method for unmanned tracked vehicles based on adaptive model prediction, in order to solve the above-mentioned technical problems.

[0012] This invention is implemented as follows: a method for controlling the motion of an unmanned tracked vehicle on a slope based on adaptive model prediction, comprising the following steps:

[0013] Real-time sensing and identification of the equivalent slope angle and heading angle of the slope where the unmanned tracked vehicle is located;

[0014] Based on the Kalman filter algorithm, the longitudinal drag coefficient and steering drag coefficient of the current ground are estimated online;

[0015] Based on a preset slip ratio lookup table, the slip ratio in the current and future prediction time domains is obtained, and the slip velocity correction term is obtained.

[0016] Based on the model predictive control method, an adaptive model predictive control framework is constructed according to the equivalent slope angle, heading angle, longitudinal drag coefficient, steering drag coefficient, and slip ratio.

[0017] Based on the adaptive model predictive control framework, the optimal control command is obtained by rolling optimization and then input to the vehicle actuator to achieve closed-loop trajectory tracking optimal control.

[0018] Furthermore, the steps for real-time sensing and identification of the equivalent slope angle and heading angle of the ramp where the unmanned tracked vehicle is located specifically include:

[0019] Acquire point cloud data around the vehicle, filter out non-ground point clouds, and convert to a geodetic coordinate system;

[0020] Plane fitting is performed on the transformed point cloud data to obtain the normal vector of the terrain plane;

[0021] The equivalent slope angle is calculated based on the angle between the normal vector and the vertical direction vector, and the heading angle is calculated in combination with the vehicle attitude angle.

[0022] Furthermore, the specific calculation methods for the equivalent slope angle and heading angle are as follows:

[0023] Establish a geodetic coordinate system O−XYZ: the positive X direction is due east, the positive Y direction is due north, and the positive Z direction is upward; where O is the selected starting point on the ground; ensure that the direction of the navigation coordinate system is consistent with the northeast-sky direction of the geodetic coordinate system O−XYZ.

[0024] The three rotation matrices corresponding to the geodetic coordinate system O−XYZ are as follows:

[0025]

[0026]

[0027]

[0028] Reference trajectory: For vehicle trajectory tracking control on slopes, the reference trajectory is provided by coordinate values ​​from the upper planning layer. , , ), and reference heading angle ;

[0029] Vehicle location: Real-time vehicle location information is received. ;in, To bypass The angle of rotation of the axis To bypass The angle of rotation of the axis To bypass The angle of rotation of the axis;

[0030] Acquiring point cloud data (x,y,z), in complete point cloud data The middle filter, excluding ground point clouds, selects point cloud information that shows the vehicle's direct contact with the ground and indicates ground adhesion. ;

[0031] Point cloud information in the navigation coordinate system Point cloud information converted to a geodetic coordinate system The coordinate transformation formula is as follows:

[0032]

[0033] in, Point cloud information in the transformed geodetic coordinate system (x,y,z) position coordinates. for Point cloud information in the navigation coordinate system before conversion (x,y,z), Right now This refers to the vehicle's actual location information in the geodetic coordinate system, which is fed back in real time by the navigation system.

[0034] Based on the real-time received vehicle location information Assuming the vehicle is currently traveling to coordinates );

[0035] Let the plane fitting equation be:

[0036]

[0037] in, , Let be the normal vector of the plane;

[0038] Computation point cloud Average coordinates ;

[0039] For any point The elements of the covariance matrix A are defined as the sum of the products of the coordinate deviations:

[0040]

[0041]

[0042] The eigenvalues ​​and eigenvectors of the covariance matrix A are obtained as follows:

[0043] =λ

[0044] Select the eigenvector corresponding to the eigenvalue with the smallest eigenvalue in matrix A. The normal vector of the fitted plane;

[0045] normal vector and vertical direction vector The acute angle between two vectors is the equivalent slope angle. heading angle .

[0046] Furthermore, based on the Kalman filter algorithm, the steps for online estimation of the current longitudinal drag coefficient and steering drag coefficient of the ground specifically include:

[0047] Set initial covariance matrix values :

[0048]

[0049] in, For the uncertainty of the initial linear velocity, Due to the uncertainty of the initial angular velocity, The uncertainty of the initial longitudinal friction coefficient f, The uncertainty of the initial steering drag coefficient μ;

[0050] The process noise covariance matrix Q and the observation noise covariance matrix R are defined as follows:

[0051] Q describes the uncertainty of the model, and Q is tuned through real-vehicle experiments: state vector It is 4-dimensional. yes Diagonal matrix:

[0052]

[0053] Specifically, for Kalman filter model estimation, For linear velocity model error, For angular velocity model error, The error is due to terrain variation in the friction coefficient f. The terrain variation error represents the steering drag coefficient μ.

[0054] Based on the requirements of different experimental scenarios, real vehicle experiments are conducted on the routes of the reference trajectories given by the upper planning layer. Q is adjusted according to the Kalman filter estimation effect, and finally Q is tuned.

[0055] R describes the uncertainty of sensor measurements: observation vector It is 2-dimensional, therefore It is a 2×2 diagonal matrix:

[0056]

[0057] in, The variance of the linear velocity measurement error. The variance of the angular velocity measurement error can be directly calculated based on the experimental measurement accuracy. and This yields the R matrix;

[0058] The dynamic model is constructed as follows:

[0059]

[0060] Where G is the vehicle weight, m ​​is the vehicle mass, L is the track contact length, B is the track center distance, θ is the slope angle, φ is the heading angle, and h is the center of gravity height. is the steering resistance coefficient when turning on a slope, and J is the yaw moment of inertia of the tracked vehicle.

[0061] The core states of the dynamic model are the vehicle's linear velocity v and angular velocity ω. The parameters to be estimated are the longitudinal friction coefficient f and the steering drag coefficient μ. The extended state vector is:

[0062]

[0063] The longitudinal friction coefficient f and steering resistance coefficient μ are estimated based on the extended Kalman filter.

[0064] Furthermore, the method for estimating the longitudinal friction coefficient f and the steering drag coefficient μ based on extended Kalman filtering specifically includes:

[0065] Establish the state equations:

[0066]

[0067] Discretization is performed using the forward Euler method, with a discretization time of O(n). ,get:

[0068] =

[0069] in, yes , The function, , To control the input, These are the driving forces for the left and right tracks, respectively. and It is a small amount of noise that has been added, assuming and All of these are parameters that change slowly;

[0070] Establish observation equations based on the linear velocity fed back from the encoder inside the vehicle. and angular velocity The observation equation is:

[0071]

[0072] Among them, the observation matrix , ; It is the observed noise, determined based on the accuracy errors of the linear velocity and angular velocity fed back by the encoder;

[0073] Iteration of the extended Kalman filter:

[0074] (1) Predicted state:

[0075] set up The initial state is =[0,0,0.2,0.6], The control inputs at each time step can be obtained in real time through the vehicle's actual encoder;

[0076] The state at the next moment can be predicted using the current state and the input:

[0077]

[0078] in, yes The state estimated at any time, yes Time estimation The state at any given moment, It is discrete time;

[0079] (2) Predicting covariance:

[0080] Find the Jacobian matrix of the state equation Update covariance:

[0081]

[0082] in, yes Time-varying covariance matrix yes Time estimation The covariance matrix at time t, Here is the noise covariance matrix;

[0083] (3) Calculate the Kalman gain:

[0084]

[0085] in, For the observation matrix, To observe the noise covariance matrix, for ;

[0086] (4) Update state and covariance

[0087] By correcting the predicted values ​​with the observed values, we obtain the estimated values ​​of f and μ at time k:

[0088]

[0089] in, for The status value fed back by the encoder inside the tracked vehicle at any given moment. It is an identity matrix.

[0090] Furthermore, the step of obtaining the slip rate in the current and future prediction time domains based on a preset slip rate lookup table, and thus obtaining the slip velocity correction term, specifically includes:

[0091] Collect theoretical speeds of different tracks Based on the corresponding actual slip ratio values, a slip ratio lookup table mapping "track theoretical speed - slip ratio" is constructed; in actual control, the current track theoretical speed is obtained in real time. The slip ratio for the corresponding working condition can be obtained directly by querying the slip ratio lookup table. Simultaneously, combining the theoretical speed predicted by the model predictive control method for future moments, the corresponding predicted slip ratio is retrieved in advance from the slip ratio lookup table; among which, the actual track speed... The slip ratio was obtained from real-time feedback of experimental inertial navigation. .

[0092] Furthermore, the method for determining the slip velocity correction term specifically includes:

[0093] Under typical working conditions on sloped roads, the theoretical speed of the tracks was collected simultaneously. With actual slip ratio This forms discrete data pairs:

[0094]

[0095] By using linear interpolation, discrete data is fitted into a continuous slip ratio lookup table:

[0096] in, This is the slip ratio fitting function corresponding to the slip ratio lookup table;

[0097] Based on the current theoretical speed of tracked vehicles The current slip ratio is obtained by querying the slip ratio lookup table:

[0098]

[0099] The future predicted by the model-based predictive control method at the current moment Theoretical speed of tracked vehicle The predicted slip ratio is obtained by querying the slip ratio lookup table:

[0100]

[0101] Substituting the current and predicted slip rates into the state correction model yields the corrected velocity, which is the slip velocity correction term:

[0102]

[0103] in, The slip ratio at the current moment. To predict the slip ratio at time k from the current time, The theoretical speed of the current tracked vehicle , The model predictive control method outputs the current time to predict the future. The theoretical speed of the tracked vehicle at all times The actual speed of the tracked vehicle at the current moment. Predict the actual speed of the tracked vehicle at time k in the future from the current time.

[0104] Furthermore, based on the model predictive control method, the steps for constructing an adaptive model predictive control framework according to the equivalent slope angle, heading angle, longitudinal drag coefficient, steering drag coefficient, and slip ratio specifically include:

[0105] Based on the dynamic model, the state-space expression of the model predictive control method is theoretically:

[0106]

[0107] Wherein, the state vector ; ; Driven by dual-track power, , Coordinates and heading angle in the geodetic coordinate system and For the speed and angular velocity of tracked vehicles;

[0108] Discretization is performed using the forward Euler method, with a discrete time of... ,get:

[0109]

[0110] The slope angle will be perceived and identified in real time. , Substituting the drag term into the state-space expression of the model predictive control method

[0111] Real-time identification using Kalman filtering , Substitute the drag calculation term into the state-space expression of the model predictive control method;

[0112] Based on the rolling optimization logic, { ,...., } is used to predict the speed over the next N steps; This is the current velocity value at time k, which can be fed back in real time through inertial navigation;

[0113] Based on a pre-defined slip ratio lookup table, the predicted slip ratio for the next N time steps is obtained, along with the predicted velocity correction value for the next N steps.

[0114] According to the formula Position is obtained using speed correction values. , heading angle angular velocity Correction value:

[0115]

[0116]

[0117]

[0118]

[0119] Finally, the predicted future state quantity correction value is obtained. .

[0120] Furthermore, based on the adaptive model predictive control framework, the optimal control command is obtained through rolling optimization and input to the vehicle actuator to achieve closed-loop trajectory tracking optimal control. The specific steps include:

[0121] The constraints are set as follows:

[0122]

[0123] in, The rate of change of driving force on both sides Driven by forces from both sides For the speed of the tracked vehicle, The angular velocity of the tracked vehicle;

[0124] A multi-objective weighted optimization function is constructed with the objectives of minimizing trajectory tracking deviation, controlling smoothness, and suppressing slip rate:

[0125]

[0126] in, These are the weight coefficients for each part of the objective function; ; ; Driven by dual-track power, The coordinates, heading angle, velocity, and angular velocity are given in the geodetic coordinate system. This represents the need for trajectory tracking; Limit slip ratio; To limit the size of the control input to ensure smooth control; This refers to the terminal status error.

[0127] Based on rolling optimization logic To predict the state variable correction values ​​over the next N steps, where the state variables include position, heading angle, velocity, and angular velocity, an adaptive model predictive control framework, constraints, and a multi-objective weighted optimization function are used. Rolling optimization control is then performed using a quadratic programming QP algorithm to obtain the optimal correction control value. For the optimal control quantity within the next N steps, only take This corresponds to two driving forces. Input to the vehicle actuator.

[0128] The present invention provides an unmanned tracked vehicle slope motion control method based on adaptive model prediction. This method addresses the motion control requirements of unmanned tracked vehicles in complex unstructured terrain (such as soft slopes and variable mechanical environments) and effectively solves the problems of model mismatch and accuracy degradation in the trajectory tracking process through adaptive model prediction control. Attached Figure Description

[0129] Figure 1 This is a flowchart of the trajectory tracking control logic for tracked vehicles.

[0130] Figure 2 Diagram of adaptive model prediction trajectory tracking control framework for tracked vehicles.

[0131] Figure 3 This is a schematic diagram of three-dimensional coordinate rotation.

[0132] Figure 4 This is a diagram showing the movement of a tracked vehicle on a slope (fitted slope angle).

[0133] Figure 5 The flowchart shows the Kalman filter parameter estimation process.

[0134] Figure 6 A flowchart for building the Model Predictive Controller (MPC).

[0135] Figure 7 The figure shows the simulation results of ramp trajectory tracking based on AMPC.

[0136] Figure 8 This is a velocity diagram from a simulation of 3D ramp trajectory tracking based on AMPC.

[0137] Figure 9 This is a simulation diagram of the angular velocity for 3D ramp trajectory tracking based on AMPC. Detailed Implementation

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

[0139] Because unmanned tracked vehicles often operate in complex geographical environments (such as soft slopes, sandy areas, mining areas, and wilderness), their motion characteristics are not only affected by terrain undulations but also exhibit extremely high uncertainty. Although existing research on trajectory tracking control for unmanned tracked vehicles has made some progress, its application in unstructured complex terrain (such as soft slopes and variable media road surfaces) still has significant limitations. This invention focuses on complex sloping road sections. The core issues of tracked vehicle operation on sloping terrain lie in slope perception, time-varying ground parameters, and the decrease in control accuracy caused by slippage disturbances.

[0140] The undulating slope causes frequent fluctuations in vehicle position and posture, posing a challenge to slope angle identification, which in turn affects the accuracy of dynamic compensation. With dynamic changes in road surface softness, humidity, and geological conditions, the longitudinal resistance parameter *f* and steering resistance parameter *μ* between the tracks and the ground fluctuate in real time. Traditional control methods typically use fixed-parameter models, which cannot adapt to such complex dynamic conditions, leading to control model mismatch. When tracked vehicles travel on sloping sections, insufficient traction easily causes significant slippage. This slippage disturbance is not only nonlinear but also has a highly destructive effect on the vehicle's lateral offset and longitudinal velocity, causing a rapid increase in trajectory tracking deviation and making it difficult to meet the requirements of high-precision control.

[0141] Therefore, such as Figures 1-2 As shown, this invention provides a slope motion control method for unmanned tracked vehicles based on adaptive model prediction. This method improves the accuracy of environmental perception and parameter estimation through multi-source information fusion technology, and can specifically solve the problems of inaccurate slope perception, time-varying ground parameters, and insufficient trajectory tracking accuracy caused by slippage interference on sloping road sections. The specific implementation is as follows:

[0142] S1. Real-time perception and identification of the equivalent slope angle and heading angle of the slope where the unmanned tracked vehicle is located;

[0143] S2. Based on the Kalman filter algorithm, estimate the longitudinal drag coefficient and steering drag coefficient of the current ground online;

[0144] S3. Based on the preset slip ratio lookup table, obtain the slip ratio in the current and future prediction time domains to obtain the slip velocity correction term;

[0145] S4. Based on the model predictive control method, an adaptive model predictive control framework is constructed according to the equivalent slope angle, heading angle, longitudinal drag coefficient, steering drag coefficient and slip ratio.

[0146] S5. Based on the adaptive model predictive control framework, the optimal control command is obtained by rolling optimization and then input to the vehicle actuator to achieve closed-loop trajectory tracking optimal control.

[0147] It should be noted that an unmanned tracked vehicle is a ground-based unmanned system capable of autonomous driving or remote control, moving via a tracked walking mechanism. This vehicle does not carry a driver and, through the integration of perception, decision-making, and control modules, performs transportation, exploration, operational, or combat missions in complex, unstructured environments (such as ruins, wilderness, deep snow, and soft mud). Adaptive Model Predictive Control (AMPC) is an advanced form of Model Predictive Control (MPC) that improves control accuracy and robustness by adjusting model parameters or structure in real time to cope with dynamic changes in the system, external disturbances, or uncertainties. Unlike fixed-parameter MPC, the core of AMPC lies in "adaptation," meaning the controller can dynamically optimize the predictive model and control strategy based on real-time perceived environmental or system states, ensuring high performance even in complex or time-varying environments. Ramp Motion Control refers to the automated control technology that enables unmanned vehicles to maintain their motion state (such as constant speed driving, hill start, anti-rollback, or controlled descent) in a slope environment with a certain slope (tilt angle) by acquiring real-time position and posture through a sensing system and adjusting the output of the power system and braking system. Its core objective is to overcome the influence of the gravity component along the slope on the vehicle's driving stability, trajectory tracking accuracy, and power system load.

[0148] In a preferred embodiment of the present invention, the step of real-time sensing and identification of the equivalent slope angle and heading angle of the ramp where the unmanned tracked vehicle is located, i.e., step S1, specifically includes:

[0149] S11. Acquire point cloud data around the vehicle, filter out non-ground point clouds, and convert to the geodetic coordinate system;

[0150] S12. Perform plane fitting on the converted point cloud data to obtain the normal vector of the terrain plane;

[0151] S13. Calculate the equivalent slope angle based on the angle between the normal vector and the vertical direction vector, and calculate the heading angle in combination with the vehicle attitude angle.

[0152] Specifically, the calculation methods for the equivalent slope angle and heading angle are as follows:

[0153] Establish a geodetic coordinate system O−XYZ: the positive X direction is due east, the positive Y direction is due north, and the positive Z direction is upward (vertically upward); where O is the selected starting point on the ground, and the position cannot be changed after selection; ensure that the direction of the navigation coordinate system is consistent with the northeast-sky direction of the geodetic coordinate system O−XYZ;

[0154] like Figure 3As shown, the three rotation matrices corresponding to the geodetic coordinate system O−XYZ are as follows:

[0155]

[0156]

[0157]

[0158] Reference trajectory: For vehicle trajectory tracking control on slopes, the reference trajectory is provided by coordinate values ​​from the upper-level planning layer (local planning module). , , ), and reference heading angle ;

[0159] Vehicle location: Real-time vehicle location information received from the inertial navigation RTK positioning system. ;in, To bypass The angle of rotation of the axis To bypass The angle of rotation of the axis To bypass The angle of rotation of the axis;

[0160] Acquire raw point cloud data from the lidar terminal (x,y,z), in complete point cloud data The middle filter, excluding ground point clouds, selects point cloud information that shows the vehicle's direct contact with the ground and indicates ground adhesion. ;

[0161] Because the lidar point cloud data is based on the vehicle's non-inertial coordinate system, with the tracked vehicle's center of gravity as the origin, the forward direction of the vehicle's front as the x-direction, the leftward direction of the vehicle's side as the y-direction, and the upward direction of the vehicle's roof as the z-direction; however, the trajectory tracking strategy, dynamics, and slope angle of the tracked vehicle in this embodiment of the invention are all based on the non-inertial reference system, the geodetic coordinate system. Below the (Northeastern Sky Coordinate System). Therefore, this embodiment of the invention will use point cloud information. Point cloud information converted to a geodetic coordinate system The coordinate transformation formula is as follows:

[0162]

[0163] in, For the converted point cloud information (x,y,z) position coordinates. for Point cloud information before conversion (x,y,z), Right now This refers to the real-time location information of the vehicle provided by the navigation system.

[0164] Based on the vehicle position information received in real time from the inertial navigation RTK positioning system Assuming the vehicle is currently traveling to coordinates This point is universally applicable to the current location of the tracked vehicle;

[0165] like Figure 4 As shown, let the plane fitting equation be:

[0166]

[0167] in, , Let be the normal vector of the plane;

[0168] Computation point cloud Average coordinates ;

[0169] For any point The elements of the covariance matrix A are defined as the sum of the products of the coordinate deviations:

[0170]

[0171]

[0172] The eigenvalues ​​and eigenvectors of the covariance matrix A are obtained as follows:

[0173] =λ

[0174] Select the eigenvector corresponding to the eigenvalue with the smallest eigenvalue in matrix A. The normal vector of the fitted plane;

[0175] normal vector and vertical direction vector The acute angle between two vectors is the equivalent slope angle. heading angle .

[0176] In summary, the final fitted plane equation is: The equivalent slope angle can be calculated. heading angle .

[0177] like Figure 5 As shown, in a preferred embodiment of the present invention, the step of estimating the longitudinal drag coefficient and steering drag coefficient of the current ground online based on the Kalman filter algorithm, i.e., step S2, specifically includes:

[0178] Set initial covariance matrix values :

[0179]

[0180] in, For the uncertainty of the initial linear velocity, Due to the uncertainty of the initial angular velocity, The uncertainty of the initial longitudinal friction coefficient f, The uncertainty of the initial steering drag coefficient μ;

[0181] The process noise covariance matrix Q and the observation noise covariance matrix R are defined as follows:

[0182] Q describes the uncertainty of the model, and Q is tuned through real-vehicle experiments: state vector It is 4-dimensional. yes Diagonal matrix (assuming process noise is independent for each state):

[0183]

[0184] Specifically, for Kalman filter model estimation, For linear velocity model error, For angular velocity model error, The error is due to terrain variation in the friction coefficient f. The terrain variation error represents the steering drag coefficient μ.

[0185] Based on the requirements of different experimental scenarios, real vehicle experiments are conducted on the routes of the reference trajectories given by the upper planning layer. Q is adjusted according to the Kalman filter estimation effect, and finally Q is tuned.

[0186] R describes the uncertainty of sensor measurements: observation vector It is 2-dimensional, therefore It is a 2×2 diagonal matrix (assuming that the observation noise of linear velocity and angular velocity is independent):

[0187]

[0188] in, The variance of the linear velocity measurement error. The variance of the angular velocity measurement error can be directly calculated based on the experimental measurement accuracy. and This yields the R matrix;

[0189] The dynamic model is constructed as follows:

[0190]

[0191] Where G is the vehicle weight, m ​​is the vehicle mass, L is the track contact length, B is the track center distance, θ is the slope angle, φ is the heading angle, and h is the center of gravity height. is the steering resistance coefficient when turning on a slope, and J is the yaw moment of inertia of the tracked vehicle.

[0192] The core states of the dynamic model are the vehicle's linear velocity v and angular velocity ω. The parameters to be estimated are the longitudinal friction coefficient f and the steering drag coefficient μ. The extended state vector is:

[0193]

[0194] The longitudinal friction coefficient f and steering resistance coefficient μ are estimated based on the extended Kalman filter.

[0195] In a preferred embodiment of the present invention, since the original dynamic model is nonlinear, it needs to be processed using an extended Kalman filter (EKF). Specifically, the method for estimating the longitudinal friction coefficient f and the steering drag coefficient μ based on the extended Kalman filter includes:

[0196] The state equation (prediction step) is established from the above formula #(14):

[0197]

[0198] Discretization is performed using the forward Euler method, with a discretization time of O(n). ,get:

[0199] =

[0200] in, yes , The function, , To control the input, These are the driving forces for the left and right tracks, respectively. and It is a small amount of noise that has been added, assuming and All of these are parameters that change slowly;

[0201] Establish the observation equation (update step) using the linear velocity fed back from the vehicle's internal encoder. and angular velocity The observation equation is:

[0202]

[0203] Among them, the observation matrix , ; It is the observed noise, determined based on the accuracy errors of the linear velocity and angular velocity fed back by the encoder;

[0204] Iteration of the extended Kalman filter (real-time estimation of f and μ):

[0205] (1) Predicted state:

[0206] set up The initial state is =[0,0,0.2,0.6], The control inputs (control quantities) at each time step can be obtained in real time through the vehicle's actual encoder;

[0207] The state at the next moment can be predicted using the current state and the input:

[0208]

[0209] in, yes The state estimated at any time, yes Time estimation The state at any given moment, It is discrete time;

[0210] (2) Predicting covariance:

[0211] Find the Jacobian matrix of the state equation Update covariance:

[0212]

[0213] in, yes Time-varying covariance matrix yes Time estimation The covariance matrix at time t, Here is the noise covariance matrix;

[0214] (3) Calculate the Kalman gain:

[0215]

[0216] in, For the observation matrix, To observe the noise covariance matrix, for ;

[0217] (4) Update state and covariance

[0218] By correcting the predicted values ​​with the observed values, we obtain the estimated values ​​of f and μ:

[0219]

[0220] in, for The status value fed back by the encoder inside the tracked vehicle at any given moment. It is an identity matrix.

[0221] In a preferred embodiment of the present invention, the step of obtaining the slip rate in the current and future prediction time domains based on a preset slip rate lookup table, and obtaining the slip velocity correction term, specifically step S3, includes:

[0222] To further improve the real-time performance and accuracy of slip ratio estimation, a real-vehicle test on a ramp was conducted. Under typical working conditions such as straight-line driving and ramp turning, different theoretical track speeds were collected. The corresponding actual slip ratio values ​​are used to construct a slip ratio lookup table (MAP) mapping "track theoretical speed - slip ratio"; in actual control, the current track theoretical speed is obtained in real time. The slip ratio for the corresponding working condition can be obtained directly by querying the slip ratio lookup table. Simultaneously, by combining the theoretical speed predicted for future moments using model predictive control methods, the corresponding predicted slip ratio is retrieved in advance from the slip ratio lookup table, achieving slip ratio adaptation in both the current and future dimensions. This provides a more accurate basis for model correction and optimized control; among which, the actual track speed... The slip ratio was obtained from real-time feedback of experimental inertial navigation. .

[0223] In a preferred embodiment of the present invention, the method for determining the slip velocity correction term specifically includes:

[0224] Under typical working conditions on sloped roads, the theoretical speed of the tracks was collected simultaneously. With actual slip ratio This forms discrete data pairs:

[0225]

[0226] By using linear interpolation, discrete data is fitted into a continuous slip ratio lookup table:

[0227] in, This is the slip ratio fitting function corresponding to the slip ratio lookup table;

[0228] Based on the current theoretical speed of tracked vehicles The current slip ratio is obtained by querying the slip ratio lookup table:

[0229]

[0230] The future predicted by the model-based predictive control method at the current moment Theoretical speed of tracked vehicle The predicted slip ratio is obtained by querying the slip ratio lookup table:

[0231]

[0232] Substituting the current and predicted slip rates into the state correction model yields the corrected velocity, which is the slip velocity correction term:

[0233]

[0234] in, The slip ratio at the current moment. To predict the slip ratio at time k from the current time, The theoretical speed of the current tracked vehicle , The model predictive control method outputs the current time to predict the future. The theoretical speed of the tracked vehicle at all times The actual speed of the tracked vehicle at the current moment. Predict the actual speed of the tracked vehicle at time k in the future from the current time.

[0235] like Figure 6 As shown, in a preferred embodiment of the present invention, in step S4, based on the real-time identification of f and μ at time k, the values ​​are substituted into the above formula. According to step S3, at time k, MPC will predict the future { ,...., The speed of the target is determined by looking up the table to predict the slip ratio at different times, and the speed predicted by the MPC at time k is corrected for future times. Finally, a model predictive controller (MPC) is constructed to optimize the trajectory tracking through an optimized cost function, thereby improving the accuracy of trajectory tracking. Step S4 specifically includes:

[0236] According to the formula The dynamic model, and the state-space expression of the model predictive control method (i.e., the MPC predictive model) are theoretically as follows:

[0237]

[0238] Wherein, the state vector ; ; Driven by dual-track power, , Coordinates and heading angle in the geodetic coordinate system and For the speed and angular velocity of tracked vehicles;

[0239] Discretization is performed using the forward Euler method, with a discrete time of... (Same as the discrete-time Kalman filter), we get:

[0240]

[0241] The slope angle will be perceived and identified in real time. , Substitute the drag calculation term into the state-space expression of the model predictive control method;

[0242] Real-time identification using Kalman filtering , Substituting the drag terms, R1 and R2, into the state-space expression of the model predictive control method (MPC predictive model), middle;

[0243] Rolling optimization logic based on MPC prediction model, { ,...., } is used to predict the speed over the next N steps; This is the current velocity value at time k, which can be fed back in real time through inertial navigation;

[0244] Based on the slip ratio lookup table preset in step S3 above, the predicted slip ratio for the next N time steps is obtained, as well as the predicted velocity correction value for the next N steps.

[0245] According to the formula Position is obtained using speed correction values. , heading angle angular velocity Correction value:

[0246]

[0247]

[0248]

[0249]

[0250] Finally, the predicted future state quantity correction value is obtained. In practical applications, the slope trajectory tracking results obtained using the above method are as follows: Figures 7-9 As shown.

[0251] In a preferred embodiment of the present invention, based on an adaptive model predictive control framework, the optimal control command is solved through rolling optimization and input to the vehicle actuator to achieve closed-loop trajectory tracking optimal control. Specifically, step S5 includes:

[0252] The constraints are set as follows:

[0253]

[0254] in, The rate of change of driving force on both sides Driven by forces from both sides For the speed of the tracked vehicle, The angular velocity of the tracked vehicle;

[0255] Define the objective function, aiming to minimize trajectory tracking deviation, control smoothness, and suppress slip rate, and construct a multi-objective weighted optimization function:

[0256]

[0257] in, These are the weight coefficients for each part of the objective function; ; ; Driven by dual-track power, The coordinates, heading angle, velocity, and angular velocity are given in the geodetic coordinate system. This represents the need for trajectory tracking (meeting the trajectory tracking objective); Limit slip ratio; To limit the size of the control input to ensure smooth control; For terminal state error (target requirements for trajectory tracking);

[0258] Rolling optimization logic based on the MPC prediction model To predict the state variable correction values ​​(such as position, heading angle, velocity, and angular velocity) within the next N steps, a rolling optimization control is performed based on an adaptive model predictive control framework (MPC predictive model), constraints, and a multi-objective weighted optimization function, using a quadratic programming QP algorithm to obtain the optimal correction control value. (Driving force of both tracks), for the optimal control quantity within the next N steps, only take... This corresponds to two driving forces. Input to the vehicle actuator.

[0259] In summary, compared with existing technologies, the embodiments of the present invention address the motion control requirements of unmanned tracked vehicles in complex unstructured terrain (such as soft slopes and variable mechanical environments) by effectively solving the model mismatch and accuracy degradation problems in the trajectory tracking process through adaptive model predictive control (AMPC). Specific advantages are as follows:

[0260] 1. Dynamic adaptation of ground parameters solves the model prediction bias caused by fixed parameters: Existing technologies usually default to the longitudinal drag coefficient of the ground. and steering drag coefficient As a fixed value, it is difficult to adapt to the fluctuations in mechanical properties caused by changes in road surface media (such as density, moisture content, and surface texture) in actual working environments.

[0261] Advantages of the solution provided in this embodiment of the invention: Real-time identification using the Kalman filter algorithm. and The dynamic changes of the vehicle's dynamics are incorporated into the prediction model of Model Predictive Control (MPC) in real time by acquiring time-varying parameters. This improvement enables the vehicle dynamics equations to be accurately matched with the real-time mechanical state of the unstructured ground, fundamentally reducing model prediction inaccuracies caused by environmental changes and significantly improving the robustness of trajectory tracking.

[0262] 2. Active compensation for slippage effect significantly improves the accuracy of state prediction and trajectory tracking: Existing technologies often ignore the significant track slippage phenomenon when unmanned tracked vehicles travel on soft or slippery surfaces. Their motion state equations lack slippage correction mechanisms, resulting in a serious cumulative deviation between theoretical calculation speed and actual driving speed.

[0263] The improvement of the solution provided by this invention embodiment: This invention embodiment calculates the slip ratio of both tracks in real time by fusing inertial navigation data and theoretical track speed, and introduces a preset MAP table to achieve a more accurate prediction of the slip ratio in the current and future prediction periods. By adding a slip speed correction term to the state variables of the MPC prediction model, the nonlinear interference caused by slip on the vehicle trajectory is actively compensated, effectively eliminating the cumulative error.

[0264] 3. Optimized multi-objective function constraints, taking into account both safety and stability under extreme conditions: The objective functions of existing technologies usually only focus on position deviation, heading angle deviation and the smoothness of control signals, and fail to make special designs for extreme conditions such as complex slopes or low-adhesion road surfaces.

[0265] The improvement of the solution provided in this embodiment of the invention: The AMPC objective function provided in this embodiment of the invention innovatively adds a slip ratio constraint term while ensuring trajectory tracking accuracy and control continuity. This improvement enables the controller to actively avoid high-slip conditions that may lead to vehicle instability or large-scale trajectory deviation when calculating the optimal sequence. Through this multi-objective optimization, the motion stability and path fitting accuracy of unmanned tracked vehicles in complex slope and time-varying road surface environments are significantly improved.

[0266] It should be understood that although the steps in the flowcharts of the embodiments of the present invention are shown sequentially according to the arrows, these steps are not necessarily executed in the order indicated by the arrows. Unless explicitly stated herein, there is no strict order restriction on the execution of these steps, and they can be executed in other orders. Moreover, at least some steps in each embodiment may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily completed at the same time, but can be executed at different times. The execution order of these sub-steps or stages is not necessarily sequential, but can be performed alternately or in turn with other steps or at least a portion of the sub-steps or stages of other steps.

[0267] The above embodiments merely illustrate several implementation methods of the present invention, and their descriptions are relatively specific and detailed, but they 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 all fall within the protection scope of the present invention. Therefore, the protection scope of this patent should be determined by the appended claims.

Claims

1. A method for controlling the motion of an unmanned tracked vehicle on a slope based on adaptive model prediction, characterized in that, Includes the following steps: Real-time sensing and identification of the equivalent slope angle and heading angle of the slope where the unmanned tracked vehicle is located; Based on the Kalman filter algorithm, the longitudinal drag coefficient and steering drag coefficient of the current ground are estimated online; Based on a preset slip ratio lookup table, the slip ratio in the current and future prediction time domains is obtained, and the slip velocity correction term is obtained. Based on the model predictive control method, an adaptive model predictive control framework is constructed according to the equivalent slope angle, heading angle, longitudinal drag coefficient, steering drag coefficient, and slip ratio. Based on the adaptive model predictive control framework, the optimal control command is obtained by rolling optimization and then input to the vehicle actuator to achieve closed-loop trajectory tracking optimal control.

2. The method for controlling the motion of unmanned tracked vehicles on slopes based on adaptive model prediction according to claim 1, characterized in that, The steps for real-time sensing and identification of the equivalent slope angle and heading angle of the ramp where the unmanned tracked vehicle is located specifically include: Acquire point cloud data around the vehicle, filter out non-ground point clouds, and convert to a geodetic coordinate system; Plane fitting is performed on the transformed point cloud data to obtain the normal vector of the terrain plane; The equivalent slope angle is calculated based on the angle between the normal vector and the vertical direction vector, and the heading angle is calculated in combination with the vehicle attitude angle.

3. The method for controlling the motion of unmanned tracked vehicles on slopes based on adaptive model prediction according to claim 2, characterized in that, The specific calculation methods for the equivalent slope angle and heading angle are as follows: Establish a geodetic coordinate system O−XYZ: the positive X direction is due east, the positive Y direction is due north, and the positive Z direction is upward; where O is the selected starting point on the ground; ensure that the direction of the navigation coordinate system is consistent with the northeast-sky direction of the geodetic coordinate system O−XYZ. The three rotation matrices corresponding to the geodetic coordinate system O−XYZ are as follows: Reference trajectory: For vehicle trajectory tracking control on slopes, the reference trajectory is provided by coordinate values ​​from the upper planning layer. , , ), and reference heading angle ; Vehicle location: Real-time vehicle location information is received. ;in, To bypass The angle of rotation of the axis To bypass The angle of rotation of the axis To bypass The angle of rotation of the axis; Acquiring point cloud data (x,y,z), in complete point cloud data The middle filter, excluding ground point clouds, selects point cloud information that shows the vehicle's direct contact with the ground and indicates its ground adhesion. ; Point cloud information in the navigation coordinate system Point cloud information converted to a geodetic coordinate system The coordinate transformation formula is as follows: in, Point cloud information in the transformed geodetic coordinate system (x,y,z) position coordinates. for Point cloud information in the navigation coordinate system before conversion (x,y,z), Right now This refers to the vehicle's actual location information in the geodetic coordinate system, which is fed back in real time by the navigation system. Based on the real-time received vehicle location information Assuming the vehicle is currently traveling to coordinates ); Let the plane fitting equation be: in, , Let be the normal vector of the plane; Computation point cloud average coordinates ; For any point The elements of the covariance matrix A are defined as the sum of the products of the coordinate deviations: The eigenvalues ​​and eigenvectors of the covariance matrix A are obtained as follows: =λ Select the eigenvector corresponding to the eigenvalue with the smallest eigenvalue in matrix A. The normal vector of the fitted plane; normal vector and vertical direction vector The acute angle between two vectors is the equivalent slope angle. heading angle .

4. The method for controlling the motion of unmanned tracked vehicles on slopes based on adaptive model prediction according to claim 3, characterized in that, The steps for online estimation of the longitudinal drag coefficient and steering drag coefficient of the current ground based on the Kalman filter algorithm specifically include: Set initial covariance matrix values : in, For the uncertainty of the initial linear velocity, For the uncertainty of the initial angular velocity, The uncertainty of the initial longitudinal friction coefficient f, The uncertainty of the initial steering drag coefficient μ; The process noise covariance matrix Q and the observation noise covariance matrix R are defined as follows: Q describes the uncertainty of the model, and Q is tuned through real-vehicle experiments: state vector It is 4-dimensional. yes Diagonal matrix: Specifically, for Kalman filter model estimation, For linear velocity model error, For angular velocity model error, The error is due to terrain variation in the friction coefficient f. The terrain variation error represents the steering drag coefficient μ. Based on the requirements of different experimental scenarios, real vehicle experiments are conducted on the routes of the reference trajectories given by the upper planning layer. Q is adjusted according to the Kalman filter estimation effect, and finally Q is tuned. R describes the uncertainty of sensor measurements: observation vector It is 2-dimensional, therefore It is a 2×2 diagonal matrix: in, The variance of the linear velocity measurement error. The variance of the angular velocity measurement error can be directly calculated based on the experimental measurement accuracy. and This yields the R matrix; The dynamic model is constructed as follows: Where G is the vehicle weight, m ​​is the vehicle mass, L is the track contact length, B is the track center distance, θ is the slope angle, φ is the heading angle, and h is the center of gravity height. is the steering resistance coefficient when turning on a slope, and J is the yaw moment of inertia of the tracked vehicle. The core states of the dynamic model are the vehicle's linear velocity v and angular velocity ω. The parameters to be estimated are the longitudinal friction coefficient f and the steering drag coefficient μ. The extended state vector is: The longitudinal friction coefficient f and steering resistance coefficient μ are estimated based on the extended Kalman filter.

5. The method for controlling the motion of unmanned tracked vehicles on slopes based on adaptive model prediction according to claim 4, characterized in that, The methods for estimating the longitudinal friction coefficient f and the steering drag coefficient μ based on extended Kalman filtering specifically include: Establish the state equations: Discretization is performed using the forward Euler method, with a discretization time of O(n). ,get: = in, yes , The function, , To control the input, These are the driving forces for the left and right tracks, respectively. and It is a small amount of noise that has been added, assuming and All of these are parameters that change slowly; Establish observation equations based on the linear velocity fed back from the encoder inside the vehicle. and angular velocity The observation equation is: Among them, the observation matrix , ; It is the observed noise, determined based on the accuracy errors of the linear velocity and angular velocity fed back by the encoder; Iteration of the extended Kalman filter: (1) Predicted state: set up The initial state is =[0,0,0.2,0.6], Control inputs at each time step can be obtained in real time through feedback from the vehicle's actual encoder; The state at the next moment can be predicted using the current state and the input: in, yes The state estimated at any time, yes Time estimation The state at any given moment, It is discrete time; (2) Predicting covariance: Find the Jacobian matrix of the state equation Update covariance: in, yes Time-varying covariance matrix yes Time estimation The covariance matrix at time t, Here is the noise covariance matrix; (3) Calculate the Kalman gain: in, For the observation matrix, To observe the noise covariance matrix, for ; (4) Update state and covariance By correcting the predicted values ​​with the observed values, we obtain the estimated values ​​of f and μ at time k: in, for The status value fed back by the encoder inside the tracked vehicle at any given moment. It is an identity matrix.

6. The method for controlling the motion of unmanned tracked vehicles on slopes based on adaptive model prediction according to claim 5, characterized in that, The steps for obtaining the slip rate correction term based on a preset slip rate lookup table, which retrieves the slip rate in the current and future prediction time domains, specifically include: Collect theoretical speeds of different tracks Based on the corresponding actual slip ratio values, a slip ratio lookup table mapping "track theoretical speed - slip ratio" is constructed; in actual control, the current track theoretical speed is obtained in real time. The slip ratio for the corresponding working condition can be obtained directly by querying the slip ratio lookup table. Simultaneously, combining the theoretical speed predicted by the model predictive control method for future moments, the corresponding predicted slip ratio is retrieved in advance from the slip ratio lookup table; among which, the actual track speed... The slip ratio was obtained from real-time feedback of experimental inertial navigation. .

7. The method for controlling the motion of unmanned tracked vehicles on slopes based on adaptive model prediction according to claim 6, characterized in that, The method for determining the slip velocity correction term specifically includes: Under typical working conditions on sloped roads, the theoretical speed of the tracks was collected simultaneously. With actual slip ratio This forms discrete data pairs: By using linear interpolation, discrete data is fitted into a continuous slip ratio lookup table: in, This is the slip ratio fitting function corresponding to the slip ratio lookup table; Based on the current theoretical speed of tracked vehicles The current slip ratio is obtained by querying the slip ratio lookup table: The future predicted by the model-based predictive control method at the current moment Theoretical speed of tracked vehicle The predicted slip ratio is obtained by querying the slip ratio lookup table: Substituting the current and predicted slip rates into the state correction model yields the corrected velocity, which is the slip velocity correction term: in, The slip ratio at the current moment. To predict the slip ratio at time k from the current time, The theoretical speed of the current tracked vehicle , The model predictive control method outputs the current time to predict the future. The theoretical speed of the tracked vehicle at all times The actual speed of the tracked vehicle at the current moment. Predict the actual speed of the tracked vehicle at time k in the future from the current time.

8. The method for controlling the motion of unmanned tracked vehicles on slopes based on adaptive model prediction according to claim 7, characterized in that, The steps for constructing an adaptive model predictive control framework based on the model predictive control method, using equivalent slope angle, heading angle, longitudinal drag coefficient, steering drag coefficient, and slip ratio, specifically include: According to the dynamic model in Equation 12, the state-space expression of the model predictive control method is theoretically: Wherein, the state vector ; ; Driven by dual-track power, , Coordinates and heading angle in the geodetic coordinate system and For the speed and angular velocity of tracked vehicles; Discretization is performed using the forward Euler method, with a discrete time of... ,get: The slope angle will be perceived and identified in real time. , Substituting the drag term into the state-space expression of the model predictive control method Real-time identification using Kalman filtering , Substitute the drag calculation term into the state-space expression of the model predictive control method; Based on the rolling optimization logic, { ,...., } is used to predict the speed over the next N steps; This is the current velocity value at time k, which can be fed back in real time through inertial navigation; Based on a pre-defined slip ratio lookup table, the predicted slip ratio for the next N time steps is obtained, along with the predicted velocity correction value for the next N steps. According to the formula Position is obtained using speed correction values. , heading angle angular velocity Correction value: Finally, the predicted future state quantity correction value is obtained. .

9. The method for controlling the motion of unmanned tracked vehicles on slopes based on adaptive model prediction according to claim 8, characterized in that, Based on the adaptive model predictive control framework, the steps of solving for the optimal control command through rolling optimization and inputting it to the vehicle actuators to achieve closed-loop trajectory tracking optimal control include: The constraints are set as follows: in, The rate of change of driving force on both sides Driven by forces from both sides For the speed of the tracked vehicle, The angular velocity of the tracked vehicle; A multi-objective weighted optimization function is constructed with the objectives of minimizing trajectory tracking deviation, controlling smoothness, and suppressing slip rate: in, These are the weight coefficients for each part of the objective function; ; ; Driven by dual-track power, The coordinates, heading angle, velocity, and angular velocity are given in the geodetic coordinate system. This represents the need for trajectory tracking; Limit slip ratio; To limit the size of the control input to ensure smooth control; This refers to the terminal status error. Based on rolling optimization logic To predict the state variable correction values ​​over the next N steps, where the state variables include position, heading angle, velocity, and angular velocity, an adaptive model predictive control framework, constraints, and a multi-objective weighted optimization function are used. Rolling optimization control is then performed using a quadratic programming QP algorithm to obtain the optimal correction control value. For the optimal control quantity within the next N steps, only take This corresponds to two driving forces. Input to the vehicle actuator.

Images