A single-soldier power-assisted tractor driving control method

Abstract

The application discloses a single-soldier-assisted traction vehicle driving control method, which comprises collecting sensor information, analyzing travel intention, predicting travel trajectory, solving control quantity and executing driving motor, and the collected sensor information comprises a tension sensor channel and a vehicle body state sensing channel; a traction force main direction angle and a traction force are calculated according to the data of the tension sensor channel; a motion trajectory is predicted by applying a space-time synchronous prediction technology; a traction vector is calculated by fusing the traction direction angle and terrain factors through the collected sensor information; basic control quantity is obtained through a solver, and basic torque is calculated; total torque resisting interference is calculated by combining a sliding mode control item and a robust compensation item; left and right wheel torques are distributed according to a steering angle, and are transmitted to a motor driving system. The single-soldier-assisted traction vehicle is convenient for adapting to different working conditions under different terrain conditions, and the precision of control under different working conditions is improved.

Description

Technical Field

[0001] This invention belongs to the technical field of vehicle drive control, and relates to a drive control method for a single-soldier assisted tractor. Background Technology

[0002] A soldier-assisted tow vehicle is a small, mechanized transport device designed specifically for individual soldiers. It is primarily used to assist soldiers in carrying heavy loads, traversing complex terrain, or performing tactical missions. Its core function is to reduce the physical burden on soldiers in complex environments while increasing their speed and agility. In recent years, with the application of artificial intelligence, robotics, and new materials, the functionality and performance of soldier-assisted tow vehicles have been significantly improved.

[0003] Existing single-soldier assisted towing vehicles often use sensor output signals to directly control motor output, which makes the system prone to vibration and instability. It lacks differentiation between human force and terrain interference, resulting in stiff control, overly fast response, and inability to adapt to different working conditions (such as uphill, downhill or emergency stop). Summary of the Invention

[0004] To address the aforementioned problems, this invention proposes a drive control method for a single-soldier assisted tractor, which effectively solves the problems in the prior art.

[0005] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0006] A drive control method for a single-soldier assisted tractor includes:

[0007] Sensor information is collected, including a tension sensing channel and a vehicle body state perception channel. The tension sensing channel is equipped with a six-axis high dynamic force sensor to capture the micro-strain signal of the traction rod in three-axis space in real time and eliminate noise through adaptive Kalman filtering. The vehicle body state perception channel includes a wheel system encoder and an IMU inertial unit, and performs weighted fusion of multi-source sensors.

[0008] The driving intention is analyzed, and the main traction direction angle and traction force are calculated based on the data from the tension sensor channel. Noise is eliminated by sliding window filtering.

[0009] The trajectory prediction uses spatiotemporal synchronous prediction technology to predict the motion trajectory, generates a time-continuous motion trajectory in Cartesian space, and smooths the path turning points using a third-order Bézier curve.

[0010] The control variables are solved by fusing sensor information with traction direction angle and terrain factors to calculate the traction vector, performing vehicle dynamics modeling, and establishing an objective function with the goal of minimizing trajectory deviation, torque fluctuation, and speed deviation. The control problem is transformed into a QP problem, and the basic control variables are obtained through the OSQP solver, and the basic torque is calculated.

[0011] The drive motor executes the calculation of the total anti-interference torque by integrating the base torque and sliding mode control terms, and distributes the torque of the left and right wheels according to the steering angle, and transmits it to the motor drive system.

[0012] Optionally, the adaptive Kalman filtering noise cancellation includes:

[0013] Based on the state at the previous moment, inputting the micro-strain signal and process noise, the state at the current moment is predicted:

[0014]

[0015] in, Let k be the state vector at time k. Let k-1 be the state vector. Here is the state transition matrix. To control the input items, This is process noise;

[0016] Obtain real-time observations and compare them with predicted observations:

[0017]

[0018] in, Let k be the observation value at time k. For predicted values, To observe noise.

[0019] Real-time correction is achieved through the innovation vector, accurately reflecting the transient characteristics of changes in the direction and intensity of the force applied by the tractioner:

[0020]

[0021] in, This is the optimal estimate at time k. Let K be the Kalman gain at time k.

[0022] Optionally, the multi-source sensing weighted fusion is as follows:

[0023]

[0024]

[0025] in, In a state of fusion, For individual state coefficients, It uses a single sensor to sense the status of the tractor.

[0026] Optionally, the principal direction angle of the traction force With traction for:

[0027]

[0028]

[0029] Sliding window filtering to eliminate noise:

[0030]

[0031]

[0032] in, The traction force is along the y-axis. The traction force along the x-axis. To handle the after-force, This represents the total amount of data in the sliding window filter. Weights for other time points, For other moments of force, This represents the weight decay rate.

[0033] Optionally, the predicted motion trajectory predicts the motion speed as follows:

[0034]

[0035] in, Let be the velocity at time t. , Let m be the speed at the current moment, and m be the mass of the tractor. For terrain compensation angle;

[0036]

[0037] in, For trajectory coordinates, , This is the initial position.

[0038]

[0039] in, For IMU pitch angle, This refers to the IMU roll angle.

[0040] In the trajectory prediction step, the path turning points are smoothed using a third-order Bézier curve:

[0041]

[0042] in, For curve parameters, Starting point , , To predict the endpoint, This is the curvature control point.

[0043] Optionally, the traction vector is:

[0044]

[0045] The objective function is:

[0046]

[0047] in, Let be the objective function. To preset the limit on the rate of change of motor torque, For actual location, For reference position, For actual speed, For reference speed, For trajectory weights, Speed ​​weights;

[0048] Solver outputs initial control solution rate With steering angle Thus, acceleration is obtained. driving force With motor base torque :

[0049]

[0050]

[0051]

[0052] in, To control the cycle, For rolling resistance, This refers to the transmission ratio from the motor to the wheels. For transmission efficiency, The radius is the wheel radius.

[0053] Optionally, the total torque for:

[0054]

[0055]

[0056] in, Based on the base torque, For sliding mode control items, This represents the speed error.

[0057] Optionally, the distribution of torque between the left and right wheels according to the steering angle includes:

[0058] Vehicle turning radius ,

[0059] in, The distance from a person to a car wheel;

[0060] Left and right wheel turning radius distribution:

[0061]

[0062]

[0063] in, The wheelbase is the distance between the left and right wheels.

[0064] The torque distribution between the left and right wheels is as follows:

[0065] The torque distribution between the left and right wheels is as follows:

[0066] Revolver :

[0067]

[0068] Right wheel :

[0069] .

[0070] Compared with the prior art, the present invention has the following advantages: by introducing filtering techniques such as adaptive Kalman filtering and sliding window filtering to process the data, and by using model prediction technology to achieve real-time matching of vehicle motion characteristics, rolling optimization of driving routes and dynamic adjustment of speed curves, the control signal is transmitted to the drive execution. Combined with sliding mode control, speed fluctuations are quickly suppressed, making it easier to adapt to different working conditions and improving the accuracy of control under different working conditions. Detailed Implementation

[0071] The technical solutions in the embodiments of the present invention will be clearly and completely described below. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0072] This invention discloses a drive control method for a single-soldier assisted tractor, comprising:

[0073] The system collects sensor information, including a tension sensing channel and a vehicle status perception channel. The tension sensing channel is equipped with a six-axis high-dynamic force sensor to capture the micro-strain signal of the traction rod in three-axis space in real time. Noise is eliminated by adaptive Kalman filtering. The vehicle status perception channel includes a wheel system encoder and an IMU inertial unit, and performs weighted fusion of multi-source sensors.

[0074] The driving intention is analyzed, and the main traction direction angle and traction force are calculated based on the data from the tension sensor channel. Noise is eliminated by sliding window filtering.

[0075] The trajectory prediction uses spatiotemporal synchronous prediction technology to predict the motion trajectory, generates a time-continuous motion trajectory in Cartesian space, and smooths the path turning points using a third-order Bézier curve.

[0076] The control variables are solved by fusing sensor information with traction direction angle and terrain factors to calculate the traction vector, performing vehicle dynamics modeling, and establishing an objective function with the goal of minimizing trajectory deviation, torque fluctuation, and speed deviation. The control problem is transformed into a QP problem, and the basic control variables are obtained through the OSQP solver, and the basic torque is calculated.

[0077] Drive torque anti-interference control integrates the base torque and sliding mode control terms to calculate the total anti-interference torque, distributes the torque to the left and right wheels according to the steering angle, and transmits it to the motor drive system.

[0078] Specifically, the system takes raw sensor data as input and starts collecting information from the raw sensor data. It works synchronously through dual parallel processing channels. The tension sensing channel captures the force on the tractor in real time, while the vehicle status sensing channel captures the vehicle's attitude, motion parameters, and terrain environment in real time.

[0079] Sensor data is fed into the driving intention analysis and driving trajectory prediction section. By calculating the force and direction of the user's traction, the user's traction intention is analyzed and a predicted trajectory is generated. An objective function is established to minimize trajectory deviation, torque fluctuation, and speed deviation, thereby optimizing the driving route and dynamically adjusting the speed curve.

[0080] The calculated base torque is used to quickly suppress speed fluctuations through sliding mode control, and the tractor is precisely controlled by controlling the distribution of wheel torque to the motor.

[0081] The tension sensing channel is equipped with a six-axis high-dynamic force sensor to capture micro-strain signals of the traction rope in X / Y / Z three-axis space in real time. The collected data includes... , , , , , Mechanical vibration noise is eliminated through adaptive Kalman filtering. Based on the state at the previous moment, the current state is predicted by inputting micro-strain signals and process noise.

[0082]

[0083] in, Let k be the state vector at time k. Let k-1 be the state vector. Here is the state transition matrix. To control the input items, This is process noise;

[0084] Obtain real-time observations and compare them with predicted observations:

[0085]

[0086] in, Let k be the observation value at time k. For predicted values, To observe noise.

[0087] Real-time correction is achieved through the innovation vector, accurately reflecting the transient characteristics of changes in the direction and intensity of the force applied by the tractioner:

[0088]

[0089] in, This is the optimal estimate at time k. Let K be the Kalman gain at time k.

[0090] Among the feasible approaches, for terrain vibration and process noise Covariance set Observation noise Covariance set Outputs 50 sets of filtered data per second, Kalman gain A dynamic adjustment mechanism, based on the signal-to-noise ratio (SNR), ensures a directional angle accuracy of ±0.5° when the SNR > 20dB. When SNR < 10dB, .

[0091] The vehicle state perception channel integrates a multi-source sensor fusion network, including a wheel encoder for real-time monitoring of the two wheel rotational speeds (W) and driving force feedback, and an IMU (Inertial Measurement Unit) outputting triaxial acceleration and angular velocity. The IMU unit detects the vehicle's pitch angle. With roll angle The feedback from both states is then merged under different operating conditions to form the current vehicle state:

[0092]

[0093]

[0094] in, In a state of fusion, For individual state coefficients, It uses a single sensor to sense the status of the tractor. For example,

[0095]

[0096] in, , , Represents the IMU output acceleration. and This represents the output speed of the left and right wheels of the wheel train encoder.

[0097] The dual-channel data are deeply fused under the control of the spatiotemporal synchronization engine, generating an environmental state vector within the cycle and outputting it to the next level.

[0098] The driving intention analysis section receives data streams from the six-axis force sensor and calculates the principal traction direction angle. With traction for:

[0099]

[0100]

[0101] Eliminating interference from muscle micro-tremors through sliding window filtering:

[0102]

[0103]

[0104] in, The traction force is along the y-axis. The traction force along the x-axis. To handle the after-force, This represents the total amount of data in the sliding window filter. Weights for other time points, For other moments of force, This represents the weight decay rate.

[0105] Considering the specific usage environment, taking into account body micro-vibrations (typical frequency 2-5Hz), and in order to eliminate transient noise interference such as wind speed, a sliding window filter is used (window size...). Signal preprocessing is performed. Based on this, user intent is identified by analyzing the rate of change of tension. (The sentence fragment about tension change rate appears to be incomplete and lacks context.) At that time, the traction mode is identified as "steady-state traction + evasive steering".

[0106] The trajectory prediction part employs a parametric trajectory description method to generate time-continuous motion trajectories in Cartesian space. The predicted motion velocity is:

[0107]

[0108] in, Let be the velocity at time t. , Let m be the speed at the current moment, and m be the mass of the tractor. For terrain compensation angle;

[0109] Predicting motion paths:

[0110]

[0111] in, For trajectory coordinates, , This is the initial position;

[0112]

[0113] in, For IMU pitch angle, This refers to the IMU roll angle.

[0114] A path is generated based on the predicted motion trajectory, and the path inflection points are smoothed using a third-order Bézier curve:

[0115]

[0116] in, For curve parameters, Starting point , , To predict the destination of the path, As the curvature control point, Determine the shape of the curve.

[0117] In the path smoothing process of the tractor, the generation of control points for the third-order Bézier curve follows these rules: starting point positioning. Directly retrieve the current position coordinates of the vehicle body ( , This point serves as the starting point of the curve, ensuring a continuous path without abrupt changes. The endpoint is determined. Take the predicted target point of the self-propelled trajectory prediction part.

[0118] Curvature control point optimization is derived in reverse based on path curvature characteristics, such as... :

[0119]

[0120]

[0121] in, The direction vector of the target point. It is the inverse proportionality coefficient of curvature. For path curvature, The possible value is 0.5. Based on a three-point curvature estimation algorithm.

[0122] The control quantity solution part obtains the principal direction angle of the traction force. With terrain compensation angle Composite traction vector:

[0123]

[0124] Calculations based on vehicle kinematics model:

[0125]

[0126]

[0127]

[0128] The tracking trajectory is predicted using a model-based method, and the tracking trajectory is corrected in real time, with a preset limit on the rate of change of motor torque. To prevent excessive rotation, a discrete state [P(0),v(0),u(0),...,P(t),v(t),u(t)] is defined with this objective in mind. T As an optimization vector, the objective function is discretized into a QP problem, and the objective function is designed. ,

[0129]

[0130] in, Let be the objective function. To preset the limit on the rate of change of motor torque, For actual location, For reference position, For actual speed, For reference speed, For trajectory weights, Speed ​​weights;

[0131] Based on the linearization of the kinematic model, we can obtain:

[0132]

[0133] set up for:

[0134]

[0135] set up for:

[0136]

[0137] Discretization yields:

[0138]

[0139] in This represents the state quantity at time k+1. This represents the state quantity at time k. Represents the sampling period. This represents the control quantity at time k.

[0140] From the above equation, we can obtain the discrete state-space equation:

[0141]

[0142] Where I represents the identity matrix

[0143] Considering the specific operating environment, the maximum torque and maximum steering angle of the motor, and the need to prevent vehicle rollover, constraints are set. , .

[0144] The solution is obtained by a solver, and the output is the control variable velocity. With steering angle acceleration Can be used at the current speed and the target speed at the next moment The driving force was calculated. Through acceleration Obtained through driving force Obtaining the wheel radius Then the motor torque can be calculated. :

[0145]

[0146]

[0147]

[0148] in, To control the cycle, For rolling resistance, This refers to the transmission ratio from the motor to the wheels. For transmission efficiency, The radius of the wheel;

[0149] To prevent abnormal output from causing the single-soldier assisted tractor to lose control, a two-level disturbance suppression system is incorporated, and a sliding surface is designed:

[0150]

[0151] in, For speed error, speed error = desired speed - actual speed, parameters The error convergence rate is determined, and it is expected that the speed error will converge to within 5% of its initial value within about 0.5 seconds. Therefore, λ = 3 / 0.5 ≈ 6 is chosen.

[0152] The total torque output of the motor is:

[0153]

[0154] in, Based on the base torque, This is a sliding mode control term with a fast response speed fluctuation and a response time of <100ms. Sliding mode control gain. To ensure that the system state converges to the sliding surface within a finite time, the values ​​are adjusted experimentally from large to small until the system stabilizes.

[0155] The turning radius of the vehicle can be calculated by calculating the steering angle.

[0156]

[0157] in, The distance from a person to a car wheel;

[0158] Therefore, the turning radii of the left and right wheels can be derived:

[0159]

[0160]

[0161] in, The wheelbase is the distance between the left and right wheels.

[0162] Torque is distributed by the turning radius of the left and right wheels:

[0163] The torque distribution between the left and right wheels is as follows:

[0164] Revolver :

[0165]

[0166] Right wheel :

[0167] .

[0168] Then, the torque of the left and right motors is transmitted to the execution output system.

[0169] Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art can still modify the technical solutions described in the foregoing embodiments or make equivalent substitutions for some of the technical features. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A drive control method for a single-soldier assisted tractor, characterized in that, include: The system collects sensor information, including a tension sensing channel and a vehicle body state perception channel. The tension sensing channel is equipped with a six-axis high-dynamic force sensor to capture the micro-strain signal of the traction rope in three-axis space in real time and eliminate noise through adaptive Kalman filtering. The vehicle body state perception channel includes a wheel system encoder and an IMU inertial unit, and performs weighted fusion of multi-source sensors. The driving intention is analyzed, and the main traction direction angle and traction force are calculated based on the data from the tension sensor channel. Noise is eliminated by sliding window filtering. The trajectory prediction uses spatiotemporal synchronous prediction technology to predict the motion trajectory, generates a time-continuous motion trajectory in Cartesian space, and smooths the path turning points using a third-order Bézier curve. The control variables are solved by fusing sensor information with traction direction angle and terrain factors to calculate the traction vector, performing vehicle dynamics modeling, and establishing an objective function with the goal of minimizing trajectory deviation, torque fluctuation, and speed deviation. The control problem is transformed into a QP problem, and the basic control variables are obtained through the OSQP solver, and the basic torque is calculated. The drive motor executes the calculation of the total anti-interference torque by integrating the basic torque and the sliding mode control term, and distributes the torque of the left and right wheels according to the steering angle and transmits it to the motor drive system. The main direction angle of the traction force With traction for: Sliding window filtering to eliminate noise: in, The traction force is along the y-axis. The traction force along the x-axis. To handle the after-force, This represents the total amount of data in the sliding window filter. Weights for other time points, For other moments of force, This represents the weight decay rate.

2. The drive control method for a single-soldier assisted tractor according to claim 1, characterized in that: The adaptive Kalman filter noise cancellation includes: Based on the state at the previous moment, inputting the micro-strain signal and process noise, the state at the current moment is predicted: in, Let k be the state vector at time k. Let k-1 be the state vector. Here is the state transition matrix. To control the input items, This is process noise; Obtain real-time observations and compare them with predicted observations: in, Let k be the observation value at time k. For predicted values, To observe noise; Real-time correction is achieved through the innovation vector, accurately reflecting the transient characteristics of changes in the direction and intensity of the force applied by the tractioner: in, This is the optimal estimate at time k. Let K be the Kalman gain at time k.

3. The drive control method for a single-soldier assisted tractor according to claim 2, characterized in that: The multi-source sensing weighted fusion is as follows: in, In a state of fusion, For individual state coefficients, It uses a single sensor to sense the status of the tractor.

4. The drive control method for a single-soldier assisted tractor according to claim 1, characterized in that: The predicted motion trajectory predicts the motion speed as follows: in, Let be the velocity at time t. , Let m be the speed at the current moment, and m be the mass of the tractor. For terrain compensation angle; in, For trajectory coordinates, , This is the initial position; in, For IMU pitch angle, For IMU roll angle; In the trajectory prediction step, the path turning points are smoothed using a third-order Bézier curve: in, For curve parameters, Starting from, To predict the endpoint, This is the curvature control point.

5. The drive control method for a single-soldier assisted tractor according to claim 4, characterized in that: The traction vector is: The objective function is: in, Let be the objective function. To preset the limit on the rate of change of motor torque, For actual location, For reference position, For actual speed, For reference speed, For trajectory weights, Speed ​​weights; Solver outputs initial control solution rate With steering angle Thus, acceleration is obtained. driving force With motor base torque : in, To control the cycle, For rolling resistance, This refers to the transmission ratio from the motor to the wheels. For transmission efficiency, The radius is the wheel radius.

6. The drive control method for a single-soldier assisted tractor according to claim 5, characterized in that: The total torque for: in, Based on the base torque, For sliding mode control items, For sliding surface, This represents the speed error.

7. The drive control method for a single-soldier assisted tractor according to claim 5, characterized in that: The method of distributing torque to the left and right wheels according to the steering angle includes: Vehicle turning radius , in, The distance from a person to a car wheel; Left and right wheel turning radius distribution: in, The wheelbase is the distance between the left and right wheels. The torque distribution between the left and right wheels is as follows: Revolver : Right wheel : 。

Images