Path tracking control method and system based on discrete element-multi-body dynamics coupling
By combining the discrete element-multibody dynamics coupling model with Kalman filtering, the accuracy and robustness issues of path tracking for road rollers on unstructured roads were solved, achieving higher path tracking accuracy and stability.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-12-24
- Publication Date
- 2026-03-10
AI Technical Summary
When a road roller is tracking a path on an unstructured road surface, it faces large system disturbances and high time-varying uncertainty in operating conditions, making traditional control methods difficult to apply and resulting in insufficient path tracking accuracy and robustness.
A simulation model of a road roller robot is constructed using a discrete element-multibody dynamics coupled model. Combined with Kalman filtering and model predictive control algorithms, road surface features are identified through offline simulation data and mapped to Kalman filter noise parameters. The control increment is then optimized in real time to approximate the reference trajectory.
It improves the path tracking accuracy and robustness of road rollers on unstructured roads, effectively addresses process errors under complex road conditions, and enhances the accuracy and stability of path tracking.
Smart Images
Figure CN121635459A_ABST
Abstract
Description
TECHNICAL FIELD
[0001] The present application relates to the technical field of road roller robot autonomous operation, and particularly relates to a road roller robot path tracking control method based on a model predictive control algorithm. BACKGROUND
[0002] As the core construction equipment for roadbed and pavement compaction operation, the road roller plays an irreplaceable role in engineering construction. However, the autonomous road roller robot still faces significant challenges in path tracking. First, in the aspect of road roller dynamics modeling, the unstructured and highly discrete soil and stone mixed pavement makes it extremely difficult to construct a mechanism model that accurately describes the influence of the road roller robot on the roadbed. Second, in the aspect of control method, most existing results are based on the assumption of known system state and vehicle model. In actual vibration compaction operation, the road roller robot needs to run on the road surface that has not been fully compacted, is loose in structure, and has high unevenness. The system disturbance is large, and the time-varying working condition has great uncertainty, which significantly reduces the accuracy of state acquisition and kinematics modeling. The above factors together make it difficult for traditional control methods to be directly applied to the autonomous operation scene of the road roller robot. SUMMARY
[0003] The purpose of the present application is to provide a path tracking control method for the road roller robot based on discrete element-multiple body dynamics coupling, aiming at the large system disturbance and the great uncertainty of time-varying working condition when the road roller robot is working.
[0004] The purpose of the present application can be achieved by the following technical solutions: As a first aspect of the present application, a path tracking control method based on discrete element-multiple body dynamics coupling is provided, comprising the following steps: A discrete element-multiple body dynamics simulation model of the road roller robot is constructed in a dynamics simulator, and a kinematics model of the road roller robot is constructed based on the articulated structure of the road roller robot; The state quantity and control quantity of the road roller robot at the current time are acquired, the slip and settlement characteristics of the road roller robot interacting with the road surface are identified by using the simulation data of the discrete element-multiple body dynamics optimization model, and the characteristics are mapped into the noise parameters of the Kalman filter. The acquired state quantity is filtered by the Kalman filter to obtain the optimal estimation of the current system state; A model predictive control framework based on the kinematics model of the road roller robot is constructed, the turning angle at the articulated joint of the road roller robot is taken as the control quantity, and the optimal control increment of the current control period is obtained by solving online to make the model predictive state approach the reference trajectory based on the reference trajectory, the system state estimated by the Kalman filter, and the current control quantity, and the optimal control increment is applied to the road roller robot in real time.
[0005] As a preferred technical solution, the discrete element-multibody dynamics simulation model of the road roller robot is constructed as follows: Establish a 3D model of the hardware architecture of the road roller robot, including the rear body, wheels, articulated structure, front frame, eccentric blocks and vibratory roller; The 3D model of the road roller robot was imported into multibody system dynamics simulation software, and constraints and functions were set between each component; a vehicle speed function was set for the tires; for the articulated structure, the function was set using the MATLAB simulation interface; and the function for the eccentric wheel mechanism inside the vibratory roller was set as follows. To generate excitation force; In discrete element simulation software, a compacted road surface is constructed. Particle properties, number of particles, and contact parameters are set. Soil and rock particle models with different geometric shapes and sizes are randomly generated. The stacking and bonding of soil and rock particle models are set to establish a roadbed model. The vibratory roller and wheel from the multibody system dynamics simulation software were imported into the discrete element simulation model to establish a discrete element-multibody dynamics simulation model of the road roller robot.
[0006] As a preferred technical solution, the kinematic model of the road roller robot is as follows: In the formula, It is the forward speed of the vibratory roller. It is speed Components on the X-axis Indicates speed Components on the Y-axis, For the direction of the vibratory roller, Indicates the rate of change of heading angle. For the road roller robot to turn corners, It is the distance from the hinge point to the center of the vibratory roller. It is the distance from the hinge point to the center of the rear frame.
[0007] As a preferred technical solution, the noise parameter mapping of the Kalman filter is performed using the discrete element-multibody dynamics simulation model, as detailed below: Offline simulation of typical working conditions was performed using the constructed discrete element-multibody dynamics coupled model; the actual state data of the road roller robot was collected during the simulation, and the theoretical state data was calculated at the same time using the constructed kinematic model of the road roller robot, and the deviation sequence between the actual state data and the theoretical state data was calculated. The covariance matrix of the bias sequence is calculated by statistical analysis, and this covariance matrix is directly mapped to the process noise covariance matrix of the Kalman filter.
[0008] As a preferred technical solution, obtaining the optimal estimate of the current system state is specifically as follows: The kinematic model of the road roller robot is converted into a state equation as follows: wherein the state variable is , the forward speed of the vibratory roller in the X-axis and Y-axis and the heading of the vibratory roller are respectively denoted as , the control variable is , the rotation angle of the road roller robot at the hinge is denoted as , wherein is the state transition matrix; is the control input matrix; is the process noise caused by the road roller robot affected by the granular road surface, the process noise obeys the normal distribution with the expectation of 0 and the covariance matrix of Q , i.e. ; The observation vector of the road roller robot is defined as wherein is the observation value affected by the measurement noise , is the output matrix; In the state space equation, the estimated value is used to replace the actual value , and the measurement estimate is introduced, and the estimated value is fused and adjusted with the measurement estimate by the Kalman filter to obtain the state variable estimation value after Kalman filtering: wherein is the prior state estimation, is the Kalman gain, is the observation vector affected by the measurement noise, is the output matrix; The state variable estimation value after Kalman filtering is taken as the feedback input of the model predictive control, and the state equation is expressed as ; The state equation is second-order Taylor expanded at the target state and the high-order terms are ignored; then, the target state is subtracted and discretized to obtain the linear error model of the road roller robot.
[0009] As a preferred technical solution, the model predictive control is adopted to obtain the optimal control increment of the current control period, and the specific process is as follows: The control amount is converted into a control increment, and a new state amount and a new state equation are constructed; The new state amount is: The new state space expression is obtained as: Wherein, represents an extended state vector, represents k a state error vector at the moment, represents k a control error vector at the moment, , represents an extended system state matrix and a control matrix, represents a unit matrix; Based on the reference trajectory, the current system state and the control amount, the future state of the system is calculated by using the prediction model, and the optimization problem is solved online through the rolling optimization strategy, the control amount is adjusted to make the predicted state approach the expected trajectory to reduce the path tracking deviation; the obtained optimal control increment is output as the instruction in real time, and the process is repeated in each control period; the objective function in the process of solving the control parameters of the road roller robot based on the model predictive control is: Wherein, , , is a weight matrix, is a relaxation factor, is a prediction time domain, is a control time domain, is the deviation between the actual output and the reference output, is a control increment, is the minimum value of the input control, is the maximum value of the input control, is the minimum value of the input control increment, is the maximum value of the input control increment.
[0010] As a second aspect of the present application, a path tracking control system based on discrete element-multiple body dynamics coupling is provided, which executes the path tracking control method based on discrete element-multiple body dynamics coupling as described above, and comprises the following steps: The kinematic model construction module: a discrete element-multiple body dynamics simulation model of the road roller robot is constructed in the dynamics simulator, and a kinematic model of the road roller robot is constructed based on the articulated structure of the road roller robot; System state estimation module: obtain the state quantity and control quantity of the road roller robot at the current time, utilize the high-fidelity characteristics of the discrete element-multi-body dynamics model, identify the slip and settlement characteristics of the road roller robot interacting with the road surface through offline simulation data, and map the characteristics to the noise parameters of the Kalman filter, filter the obtained state quantity through the Kalman filter, and obtain the optimal estimation of the current system state; Road roller robot control module: construct a model predictive control framework based on the kinematic model of the road roller robot, based on the reference trajectory, the system state estimated by the Kalman filter, and the current control quantity, solve online to make the predicted state approach the reference trajectory to obtain the optimal control increment of the current control period, and act on the road roller robot in real time.
[0011] As a preferred technical solution, the kinematic model construction module constructs a discrete element-multi-body dynamics simulation model of the road roller robot as follows: A three-dimensional model of the hardware architecture of the road roller robot is established, including a rear vehicle body, wheels, a hinged structure, a front vehicle frame, an eccentric block and a vibrating roller; The three-dimensional model of the road roller robot is imported into a multi-body system dynamics simulation software, and the constraints and functions between each component are set; the speed function is set for the tire; for the hinged structure, the function is set as a MATLAB joint simulation interface; and the function of the eccentric mechanism in the vibrating roller is set as to generate an exciting force; In the discrete element simulation software, a compaction operation road surface is constructed, the particle attributes, the number of particles, and the contact parameters are set, different geometric shapes and sizes of soil and rock particle models are randomly generated, and the accumulation and adhesion of the soil and rock particle models are set to establish a roadbed model; The vibrating roller and the wheels in the multi-body system dynamics simulation software are imported into the discrete element simulation model to establish a discrete element-multi-body dynamics simulation model of the road roller robot; The kinematic model of the road roller robot is: In the formula, is the forward speed of the vibrating roller, is the speed is the component of the speed in the X-axis, represents the speed is the component of the speed in the Y-axis, is the heading of the vibrating roller, represents the rate of change of the heading angle, is the turning angle of the road roller robot, is the distance from the hinged point to the center of the vibrating roller, is the distance from the hinged point to the center of the rear vehicle frame.
[0012] As a preferred technical solution, the system state estimation module obtains the optimal estimate of the current system state, specifically as follows: Offline simulation of typical working conditions was performed using the constructed discrete element-multibody dynamics coupled model; the actual state data of the road roller robot was collected during the simulation, and the theoretical state data was calculated at the same time using the constructed kinematic model of the road roller robot, and the deviation sequence between the actual state data and the theoretical state data was calculated. Statistical analysis is performed on the deviation sequence to calculate its covariance matrix, and this covariance matrix is directly mapped to the process noise covariance matrix of the Kalman filter. Q ; The kinematic model of the road roller robot is transformed into the state equation as follows: Among them, the state variables are , These represent the components of the vibratory roller's forward speed along the X and Y axes, respectively, and the control variable is... , This is the state transition matrix; The disturbances experienced by the road roller operating on unstructured road surfaces are represented by the state-space equation process noise: in, This is the state transition matrix; To control the input matrix; This refers to the process noise caused by the impact of particulate road surface on the road rolling robot. It follows the condition that the expectation is 0 and the covariance matrix is Q The normal distribution, i.e. ; The observation vector of the road roller robot is defined as: in, The actual value is affected by measurement noise. The affected observations This is the output matrix; In the state-space equations, the estimated values are used. Replace actual value And introduce measurement estimation Data fusion is performed using a Kalman filter to convert the estimated values. With measurement estimation After fusion and adjustment, the state quantity estimates after Kalman filtering are obtained: in, For prior state estimation, For Kalman gain, is the observation vector affected by measurement noise, is the output matrix; the state quantity estimation value after Kalman filtering As the feedback input of model predictive control, the state equation is expressed as ; The state equation is second-order Taylor expanded at the target state and the high-order terms are ignored; then, after being subtracted from the target state and discretized, a linear error model of the road roller robot is obtained.
[0013] As a preferred technical solution, the road roller robot control module acquires the optimal control increment of the current control period, specifically as follows: The control quantity is converted into a control increment, and a new state quantity and a new state equation are constructed; The new state quantity is: The new state space expression is: wherein, represents an extended state vector, represents k a state error vector at the moment, represents k a control error vector at the moment, , represents an extended system state matrix and a control matrix, represents a unit matrix; Based on the reference trajectory, the current system state and the control quantity, the future state of the system is calculated by using the prediction model, and the optimization problem is solved online by using the rolling optimization strategy, the control quantity is adjusted to make the predicted state approach the reference trajectory to reduce the path tracking deviation; the optimal control increment obtained is output as the instruction in real time, and the process is repeated in each control period; during the process of solving the control parameters of the road roller robot by using the model predictive control, the objective function is: wherein, , , is a weight matrix, is a relaxation factor, is a prediction time domain, is a control time domain, is the deviation between the actual output and the reference output, is a control increment, is the minimum value of the input control, is the maximum value of the input control, is the minimum value of the input control increment, This is the maximum value of the input control increment.
[0014] Compared with the prior art, the present invention has the following beneficial effects: This invention addresses the difficulty of constructing mechanistic models by employing a discrete element-multibody dynamics model (DEM) for road rollers. It identifies the slippage and settlement characteristics of the interaction between the road roller and the road surface using offline simulation data and maps these characteristics to the noise parameters of a Kalman filter. Subsequently, the Kalman filter is combined with an MPC control algorithm to make the predicted state approximate the desired trajectory, thereby optimizing the control increment for the current control cycle. This invention effectively handles unstructured road conditions, solving the problems of difficult process error modeling and accurate state variable acquisition for road rollers under complex unstructured road conditions, thus improving the accuracy and robustness of path tracking. Attached Figure Description
[0015] Figure 1 This is a flowchart of the path tracking control method for a road roller robot based on a model predictive control algorithm, as described in this invention.
[0016] Figure 2 This is a diagram of the discrete element-multibody dynamics model of the road roller robot of this invention.
[0017] Figure 3 This is a kinematic model diagram of the road roller robot of the present invention.
[0018] Figure 4 This is a comparison between the actual path and the target path in an embodiment of the present invention.
[0019] Figure 5 The diagram shows a comparison between the proposed solution and the traditional MPC control, with (a) lateral error evolution and (b) heading error evolution. Detailed Implementation
[0020] The present invention will now be described in detail with reference to the accompanying drawings and specific embodiments. These embodiments are based on the technical solution of the present invention and provide detailed implementation methods and specific operating procedures. However, the scope of protection of the present invention is not limited to the following embodiments.
[0021] Example 1 like Figure 1 As shown, this invention provides a path tracking control method for a road roller robot based on a model predictive control algorithm, comprising the following steps: S1: Construct a discrete element-multibody dynamics simulation model of the road roller robot in the dynamics simulator, and construct a kinematic model of the road roller robot based on the articulated structure of the road roller robot; Specifically, by analyzing the hinge mechanism between the vibrating roller and the rear frame of the road roller robot, the geometric relationship among the vibrating roller centroid, the rear frame centroid and the hinge point is first determined, and the heading definition and the rotation angle relationship of each component are clarified. On this basis, according to the motion characteristics of the vehicle in the plane, the velocity expressions of the centroids in the longitudinal and transverse directions are derived respectively, and the correlation between the angular velocities of the front and rear bodies is established by combining the hinge constraints. Further, by comprehensively substituting factors such as geometric parameters, hinge offset angle and driving speed into the motion constraint relationship, a kinematic model is obtained, which can describe the changes in the planar position and heading of the road roller robot.
[0022] S2: Obtain the state quantity and control quantity at the current time, and use the high-fidelity characteristics of the discrete element-multiple body dynamics model to identify the slip and settlement characteristics of the road roller robot interacting with the road surface through simulation data offline, and map the characteristics to the noise parameters of the Kalman filter. The state quantity obtained is filtered by the Kalman filter.
[0023] Specifically, the discrete element-multiple body dynamic coupling model is used to perform offline simulation of typical working conditions. The road roller robot is controlled to move along the reference path, and the actual state data and theoretical state data of the robot during the simulation process are collected to calculate the deviation sequence. Since the discrete element-multiple body dynamics model includes the complex interaction between soil particles and machinery, the deviation sequence reflects the process noise of the system caused by the unstructured road surface environment. The covariance matrix of the deviation sequence is calculated by statistical analysis, and is directly used as the process noise covariance matrix of the Kalman filter. Through the constructed Kalman filter, the process noise introduced by factors such as particle disturbance, side slip and vibration impact under unstructured road conditions is considered, and the state updating process is modeled as an uncertain system with random disturbance to reflect the model bias that is difficult to accurately characterize in state prediction. Subsequently, an observation model is established based on the observation data collected by the sensor in real time, and measurement noise is introduced into the observation model to characterize the observation error caused by the sensor in the vibration environment. In each filtering period, the Kalman filter first makes a prior prediction of the current system state according to the state model, then calculates the prediction error based on the actual observation information, and determines the corresponding Kalman gain accordingly; under the action of the Kalman gain, the predicted state and the observation information are weighted and fused to obtain the optimal estimate of the current system state.
[0024] S3: Construct a model predictive control framework based on the kinematic model of the road roller robot, and solve the optimization problem by rolling online to obtain the optimal control increment of the current control period, and apply it to the road roller robot in real time to ensure accurate tracking of the actual trajectory to the reference path. Specifically, by the constructed MPC prediction model, the state quantity at the next moment can be predicted, the state quantity at the next moment is taken as the control input of the MPC prediction model, the target function is used to optimize the control increment at the current moment, only the first control increment in the optimized control sequence is applied to the road roller robot; then the MPC prediction model adjusts the state quantity according to the control increment, and a new control increment is recalculated in the next control period, and the new control increment is applied to the road roller robot again.
[0025] S4: path tracking control verification is carried out in a simulation environment, and the robustness and tracking accuracy of the controller are evaluated; Specifically, a tracking path from lane changing to a parallel road surface is designed, the straight line tracking and steering capability are evaluated, and the application is compared with a traditional model prediction control algorithm.
[0026] The specific embodiments of the application are further described below.
[0027] As shown in Figure 2 , the road roller discrete element-multibody dynamics simulation model construction specifically comprises: The rear vehicle body, wheel, hinged structure, front frame, eccentric block and vibratory roller are modeled, and the model is converted into a RecurDyn importable.x_t model file; The.x_t model file is imported into RecurDyn, and the constraints and functions between the parts in the model are set: for 1 tire, its function is set to STEP(0.5, 0, 2, 1) in RecurDyn, i.e. the vehicle speed gradually increases from zero to 1 m / s ; for the hinged structure 2, the function is set to PIN, i.e. the MATLAB joint interface; the eccentric mechanism 3 in the vibratory roller is set to to generate the exciting force.
[0028] The vibratory roller and the wheel are set to wall and the.wall file is exported; The.wall file is imported into EDEM, the particle properties, particle number and contact parameters are set, different geometric shapes and sizes of soil and rock particle models are randomly generated, the Hertz-Mindlin(no slip) model is selected to calculate the accumulation and adhesion of the soil and rock particle models, and thus the subgrade model 4 is established, so that the road roller discrete element-multibody dynamics simulation model is established in the Recurdyn software and the EDEM software.
[0029] As shown in Figure 3 , the road roller kinematics model construction method comprises: Figures and are the headings of the vibratory roller and the rear frame, respectively. is the turning angle of the compactor robot; is the center position of the vibratory roller; is the hinge point is the distance between the hinge point and the center of the vibratory roller; , are the forward speeds of the vibratory roller and the rear frame, respectively; the center position of the rear frame is ; is the distance from the hinge point to the center of the rear frame.
[0030] Assuming that the turning angle of the vehicle is kept constant, the following can be derived through the geometric characteristics of the vehicle: The front and rear parts of the compactor robot are connected through a hinge, and the hinge point is , so the relative velocity vector equation is defined as: where and are the angular velocities of the vibratory roller and the rear frame, respectively. Using the geometric relationship: the angular velocity of the compactor robot is: In summary, the kinematic model of the compactor robot can be represented as: that is: In the present application, step S2 comprises: S21: using the established discrete element-multiple body dynamic coupling model to perform offline simulation of typical working conditions. In specific implementation, the simulation environment is set to be a preset unstructured soil road surface, the compactor robot is controlled to move along a reference path, and actual state data of the robot during the simulation process is collected ; at the same time, the established kinematic model of the compactor robot is used to calculate theoretical state data under the same control instruction, so as to calculate a deviation sequence . Since the discrete element-multiple body dynamic model contains complex interaction between soil particles and machinery, the deviation sequence reflects the process noise of the unstructured road surface environment on the system . The deviation sequence E is statistically analyzed, and its covariance matrix is calculated, which is directly used as the process noise covariance matrix Q of the Kalman filter.
[0031] S22: Convert the kinematic model of the road roller robot into a state equation as follows: where the state variable is , and the control variable is , the rotation angle of the road roller robot at the hinge point; is the state transition matrix.
[0032] S23: Express the disturbance of the road roller robot working on an unstructured road surface as a state space equation process noise.
[0033] where is the state transition matrix; is the control input matrix; is the process noise of the road roller robot caused by the influence of the granular road surface, which is generated in the process of converting the state vector from to through the S21 step, and the process noise obeys a normal distribution with an expected value of 0 and a covariance matrix of Q , i.e. .
[0034] S24: Define the observation vector of the road roller robot as follows: where is the observed value affected by the measurement noise , is the output matrix.
[0035] S25: Use the estimated value instead of the actual value in the state space equation, and do the same processing to the measurement value, introducing the measurement estimate . Through the Kalman filter, the estimated value is fused and adjusted with to obtain the Kalman filtered state variable estimate as follows: where is the prior state estimate, is the Kalman gain, is the observation vector affected by the measurement noise. Taking as the feedback input of the MPC control, the state equation is represented as .
[0036] Let the system target state be The state equation is second-order Taylor expanded at the target state and high-order terms are ignored to obtain: After being subtracted from the target state and discretized, the compactor robot linear error model is obtained: wherein, , . is the control period, is the state matrix of the continuous-time system, is the unit matrix, is the control matrix; is the state error vector; is the control error vector.
[0037] In the present application, step S3 comprises: S31: To improve the stability and controllability of the underlying actuator, the control quantity is converted into a control increment, and a new state quantity and a new state equation are constructed; The new state quantity is: The new state space expression is obtained as: wherein, represents the extended state vector, represents the state error vector at the moment, k represents the control error vector at the moment, , k represents the extended system state matrix and the control matrix, represents the unit matrix. S32: Based on the reference trajectory, the current system state and the control quantity, the future state of the system is calculated using the prediction model, and the optimization problem is solved online through the rolling optimization strategy to adjust the control quantity so that the predicted state approximates the expected trajectory, thereby reducing the path tracking deviation; the optimal control increment obtained is then output as the instruction in real time, and the process is repeated in each control period.
[0038] S32: Based on the reference trajectory, the current system state and the control quantity, the future state of the system is calculated using the prediction model, and the optimization problem is solved online through the rolling optimization strategy to adjust the control quantity so that the predicted state approximates the expected trajectory, thereby reducing the path tracking deviation; the optimal control increment obtained is then output as the instruction in real time, and the process is repeated in each control period.
[0039] In constructing the model predictive control, first, the length of the prediction horizon is set to limit the number of future time points forward deduced in the optimization process; the longer the prediction horizon, the more future behavior of the system that the model can foresee. At the same time, the length of the control horizon is set to constrain the number of control amounts allowed to be solved in one optimization, and the control horizon does not exceed the prediction horizon. In the case of knowing the current system state and the current control increment, the system state at the next time point can be calculated according to the state update model, and the state sequence at each time point in the prediction horizon can be obtained by recursion.
[0040] In the process of solving the control parameters of the road roller robot according to the MPC algorithm, the objective function and the constraint condition are: wherein, , , is a weight matrix, is a relaxation factor, is a prediction horizon, is a control horizon, is a deviation between the actual output and the reference output, is a control increment, is a minimum value of the input control, is a maximum value of the input control, is a minimum value of the input control increment, is a maximum value of the input control increment.
[0041] Accurate and stable tracking of the desired path is a key requirement that must be met simultaneously in path tracking control. First, the tracking accuracy depends on the deviation between the actual position of the vehicle during driving and the reference trajectory, so the first term in the objective function is used to measure the consistency between the predicted state and the reference state to improve the fitting ability of the road roller robot to the desired path. Second, the tracking stability is closely related to the change amplitude of the steering action, and excessive steering change will lead to a decrease in the lateral stability of the vehicle, so the second term of the objective function introduces the change of the control input to suppress the sharp change of the control command and maintain the smoothness of the vehicle driving. In addition, the third term as a relaxation factor is used to ensure that a feasible solution can be obtained in each control period, thereby enhancing the robustness of the optimization process. By setting the constraint condition, the change range of the control increment and the state quantity can be limited, the control deviation caused by the uncertainty of the MPC prediction model or external disturbance can be reduced, and it can be ensured that the road roller robot system operates within a safe range.
[0042] In the present application, the path tracking verification in step S4 is specifically: Design a tracking path for lane change to parallel road surface, with an initial error of 3. m The initial roller rotation angle is set to This invention is compared with traditional MPC control. Figure 4 The invention demonstrates that it achieves higher tracking accuracy than traditional MPC control. Figure 5 As shown in (a) and (b), the present invention has a faster convergence speed and smaller error than conventional MPC control.
[0043] Example 2 As another embodiment of the present invention, this embodiment provides a path tracking control system based on discrete element-multibody dynamics coupling, characterized in that the system executes the path tracking control method for a road roller robot as described in Embodiment 1, including the following steps: Kinematic model construction module: Construct a discrete element-multibody dynamics simulation model of the road roller robot in the dynamics simulator, and construct the kinematic model of the road roller robot based on the articulated structure of the road roller robot; System state estimation module: It acquires the state and control variables of the road roller at the current moment, utilizes the high-fidelity characteristics of the discrete element-multibody dynamics model, identifies the slippage and settlement characteristics of the interaction between the road roller and the road surface through offline simulation data, maps these characteristics to the noise parameters of the Kalman filter, and filters the acquired state variables through the Kalman filter to obtain the optimal estimate of the current system state. Road roller robot control module: Construct a model predictive control framework based on the kinematic model of the road roller robot. Based on the reference trajectory, the system state estimated by Kalman filter and the current control quantity, online rolling solution is used to make the predicted state approximate the reference trajectory, so as to obtain the optimal control increment of the current control cycle and apply it to the road roller robot in real time.
[0044] The kinematics model construction module constructs the discrete element-multibody dynamics simulation model of the road roller robot as follows: Establish a 3D model of the hardware architecture of the road roller robot, including the rear body, wheels, articulated structure, front frame, eccentric blocks and vibratory roller; The 3D model of the road roller robot was imported into multibody system dynamics simulation software, and constraints and functions were set between each component; a vehicle speed function was set for the tires; for the articulated structure, the function was set using the MATLAB simulation interface; and the function for the eccentric wheel mechanism inside the vibratory roller was set as follows. To generate excitation force; In discrete element simulation software, a compacted road surface is constructed. Particle properties, number of particles, and contact parameters are set. Soil and rock particle models with different geometric shapes and sizes are randomly generated. The stacking and bonding of soil and rock particle models are set to establish a roadbed model. The vibrating roller and the wheel in the multi-body system dynamics simulation software are imported into the discrete element simulation model to establish a discrete element-multiple body dynamics simulation model of the road roller robot; The road roller robot kinematics model is: In the formula, is the forward speed of the vibrating roller, is the speed is the component of the speed on the X axis, denotes the speed is the component of the speed on the Y axis, is the heading of the vibrating roller, denotes the rate of change of the heading angle, is the turning angle of the road roller robot, is the distance from the hinge point to the center of the vibrating roller, is the distance from the hinge point to the center of the rear frame.
[0045] Further, the system state estimation module obtains an optimal estimation of the current system state, and specifically as follows: The discrete element-multiple body dynamics coupling model is used to perform offline simulation under typical working conditions; the actual state data of the road roller robot in the simulation process are collected, and the theoretical state data are calculated by the built road roller robot kinematics model, and the deviation sequence of the actual state data and the theoretical state data is calculated; The deviation sequence is statistically analyzed to calculate the covariance matrix, and the covariance matrix is directly mapped to the process noise covariance matrix of the Kalman filter Q ; The road roller robot kinematics model is converted into a state equation as follows: In the formula, the state quantity is , denotes the component of the vibrating roller forward speed on the X axis and the Y axis and the heading of the vibrating roller, the control quantity is , is the state transition matrix; The disturbance of the road roller robot working on an unstructured road surface is expressed as the process noise of the state space equation as follows: In the formula, is the state transition matrix; is the control input matrix; is the process noise caused by the influence of the granular road surface on the road roller robot, and the process noise obeys the normal distribution with the expectation of 0 and the covariance matrix of Q , that is, ; The observation vector of the road roller robot is defined as: in, The actual value is affected by measurement noise. The affected observations This is the output matrix; In the state-space equations, the estimated values are used. Replace actual value And introduce measurement estimation Data fusion is performed using a Kalman filter to convert the estimated values. With measurement estimation After fusion and adjustment, the state quantity estimates after Kalman filtering are obtained: in, For prior state estimation, For Kalman gain, The observation vector after being affected by measurement noise. This is the output matrix; The state quantity estimate after Kalman filtering As the feedback input for model predictive control, the state equation is expressed as: ; The state equation is expanded in second-order Taylor at the target state and higher-order terms are ignored. Then, it is discretized after being subtracted from the target state to obtain the linear error model of the road roller robot.
[0046] Furthermore, the road roller robot control module obtains the optimal control increment for the current control cycle, as follows: Convert the control input into a control increment, and construct new state variables and new state equations; The new state variable is: The new state-space expression is obtained as follows: in, Represents the extended-dimensional state vector. express k The state error vector at time t. express k The control error vector at time t, , Represents the state matrix and control matrix of the extended-dimensional system. Represents the identity matrix; Based on the reference trajectory, the current system state and the control amount, the future state of the system is calculated by using the prediction model, and the optimization problem is solved online by using the rolling optimization strategy to adjust the control amount to make the predicted state approach the reference trajectory to reduce the path tracking deviation; the obtained optimal control increment is output as the instruction in real time, and the process is repeated in each control period; the objective function in the process of solving the control parameters of the road roller robot by the model predictive control is: wherein, , , is a weight matrix, is a relaxation factor, is a prediction time domain, is a control time domain, is the deviation between the actual output and the reference output, is a control increment, is the minimum value of the input control, is the maximum value of the input control, is the minimum value of the input control increment, is the maximum value of the input control increment.
[0047] If the above functions are realized in the form of software function units and sold or used as independent products, they can be stored in a computer readable storage medium. Based on this understanding, the technical solutions of the present application or the parts of the prior art that essentially contribute or the parts of the technical solutions can be embodied in the form of a software product, which is stored in a storage medium and includes a plurality of instructions for causing a computer device (which can be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the method described in the embodiments of the present application. The aforementioned storage medium includes: a U disk, a mobile hard disk, a read-only memory (ROM, Read-Only Memory), a random access memory (RAM, Random Access Memory), a magnetic disk or an optical disk and various program code storage media.
[0048] The preferred embodiments of the present application are described in detail above. It should be understood that those skilled in the art can make many modifications and changes without creative labor according to the concept of the present application. Therefore, any technical solution obtained by logical analysis, reasoning or limited experiment by those skilled in the art on the basis of the prior art according to the concept of the present application shall be within the protection scope determined by the claims.
Claims
1. A path following control method based on discrete element-multiple body dynamics coupling, characterized by, The method comprises the following steps: A discrete element-multiple body dynamics simulation model of the road roller is constructed in a dynamics simulator, and a kinematics model of the road roller is constructed based on a hinged structure of the road roller; State quantities and control quantities of the road roller at a current time are obtained, a slip and settlement feature of the road roller interacting with a road surface is identified by using simulation data of the discrete element-multiple body dynamics optimization model, and the feature is mapped into noise parameters of a Kalman filter, the state quantities obtained are filtered by the Kalman filter to obtain an optimal estimation of a current system state; A model predictive control framework based on the kinematics model of the road roller is constructed, a turning angle at a hinge of the road roller is taken as a control quantity, a reference trajectory, a system state estimated by the Kalman filter and a current control quantity are used to solve online and roll to make a model predictive state approach the reference trajectory, and an optimal control increment of a current control period is obtained and applied to the road roller in real time.
2. The path following control method based on discrete element-multiple body dynamics coupling according to claim 1, characterized in that, The discrete element-multiple body dynamics simulation model of the road roller is constructed as follows: A three-dimensional model of a hardware architecture of the road roller is established, and the three-dimensional model comprises a rear vehicle body, a wheel, a hinged structure, a front vehicle frame, an eccentric block and a vibrating roller; The road roller robot three-dimensional model is imported into multi-body system dynamics simulation software, constraints and functions between various components are set, a speed function is set for the tire, a MATLAB joint simulation interface function is set for the articulated structure, and a function is set for the eccentric wheel mechanism in the vibrating roller to generate exciting force. to generate exciting force. A compaction operation road surface is constructed in a discrete element simulation software, particle attributes, a particle number and contact parameters are set, soil and stone particle models of different geometric shapes and different sizes are randomly generated, and accumulation and adhesion of the soil and stone particle models are set to establish a roadbed model; The vibrating roller and the wheel in a multiple body system dynamics simulation software are imported into the discrete element simulation model to establish the discrete element-multiple body dynamics simulation model of the road roller.
3. The path following control method based on discrete element-multiple body dynamics coupling according to claim 1, characterized in that, The kinematics model of the road roller is as follows: wherein is the forward speed of the vibratory roller, is the speed is the component in the X axis, denotes the speed is the component in the Y axis, is the heading of the vibratory roller, denotes the rate of change of the heading angle, is the turning angle of the roller robot, is the distance from the hinge point to the center of the vibratory roller, is the distance from the hinge point to the center of the rear frame.
4. The path following control method based on discrete element-multiple body dynamics coupling according to claim 1, wherein, Noise parameter mapping of the Kalman filter by using the discrete element-multiple body dynamics simulation model is as follows: An offline simulation of a typical working condition is performed by using the discrete element-multiple body dynamics coupling model constructed, actual state data of the road roller in the simulation process are collected, theoretical state data are calculated by using the kinematics model of the road roller constructed, and a deviation sequence of the actual state data and the theoretical state data is calculated; A covariance matrix of the deviation sequence is calculated by statistical analysis, and the covariance matrix is directly mapped into a process noise covariance matrix of the Kalman filter.
5. The path following control method based on discrete element-multiple body dynamics coupling according to claim 4, characterized in that, The optimal estimation of the current system state is as follows: The kinematics model of the road roller is converted into a state equation as follows: Wherein, the state quantity is , respectively represent the component of the vibration roller forward speed in the X-axis and the Y-axis and the heading of the vibration roller; the control quantity is , the turning angle of the road roller robot articulation; the state transition matrix; Disturbance of the road roller in unstructured road operation is expressed as process noise of the state space equation as follows: wherein, is a state transition matrix; is a control input matrix; is process noise caused by the road roller robot being affected by the granular road surface, the process noise is subject to the expectation being 0, the covariance matrix is Q a normal distribution, i.e. ; An observation vector of the road roller is defined as follows: wherein, is an observed value of the actual value affected by measurement noise, is an observed value of the actual value affected by measurement noise, is an output matrix; In the state space equation, the estimated value is used instead of the actual value and the measurement estimate is introduced , the estimated value is fused and adjusted with the measurement estimate by the Kalman filter, and the state quantity estimated value after Kalman filtering is obtained: wherein, is a prior state estimate, is a Kalman gain, is an observation vector affected by measurement noise, is an output matrix; The state quantity estimation value after Kalman filtering As a feedback input of model predictive control, the state equation is expressed as ; The state equation is expanded into a second-order Taylor series at a target state and high-order terms are ignored, and then the state equation is subtracted from the target state and discretized to obtain a linear error model of the road roller.
6. The path following control method based on discrete element-multiple body dynamics coupling according to claim 1, wherein, The optimal control increment of the current control period is obtained by using the model predictive control as follows: The control quantity is converted into a control increment, a new state quantity and a new state equation are constructed; The new state quantity is as follows: A new state space expression is as follows: wherein denotes the augmented state vector, denotes k the state error vector at time instant, denotes k the control error vector at time instant, , denotes the augmented system state matrix and control matrix, denotes the identity matrix; Based on the reference trajectory, the current system state and the control variable, the future state of the system is calculated using a prediction model, and the optimization problem is solved online through a rolling optimization strategy to adjust the control variable to make the predicted state approach the desired trajectory to reduce the path tracking deviation; the optimal control increment obtained is output as the instruction in real time, and this process is repeated in each control period; the objective function in the process of solving the control parameters of the road roller robot by model predictive control is: wherein, , , is a weight matrix, is a relaxation factor, is a prediction horizon, is a control horizon, is a deviation of the actual output from the reference output, is a control increment, is a minimum value of the input control, is a maximum value of the input control, is a minimum value of the input control increment, is a maximum value of the input control increment.
7. A path following control system based on discrete element - multi-body dynamics coupling, characterized by, The system performs the path tracking control method based on discrete element-multiple body dynamics coupling as claimed in any one of claims 1-6, comprising the following steps: A kinematic model construction module: a discrete element-multiple body dynamics simulation model of the road roller robot is constructed in a dynamics simulator, and a kinematic model of the road roller robot is constructed based on the articulated structure of the road roller robot; A system state estimation module: the state quantity and control variable of the road roller robot at the current time are obtained, the high-fidelity characteristics of the discrete element-multiple body dynamics model are used, the slip and settlement characteristics of the road roller robot interacting with the road surface are identified through offline simulation data, and the characteristics are mapped to the noise parameters of the Kalman filter; the obtained state quantity is filtered by the Kalman filter to obtain the optimal estimation of the current system state; A road roller robot control module: a model predictive control framework based on the kinematic model of the road roller robot is constructed, the reference trajectory, the system state estimated by the Kalman filter, and the current control variable are used to solve online and roll to make the predicted state approach the reference trajectory to obtain the optimal control increment of the current control period, and the optimal control increment is applied to the road roller robot in real time.
8. The path following control system based on discrete element-multiple body dynamics coupling of claim 7, wherein, The kinematic model construction module constructs the discrete element-multiple body dynamics simulation model of the road roller robot as follows: A three-dimensional model of the hardware architecture of the road roller robot is established, including the rear vehicle body, the wheels, the articulated structure, the front vehicle frame, the eccentric block and the vibrating roller; The road roller robot three-dimensional model is imported into multi-body system dynamics simulation software, constraints and functions between various components are set, a speed function is set for the tire, a MATLAB joint simulation interface function is set for the articulated structure, and a function is set for the eccentric wheel mechanism in the vibrating roller to generate exciting force. to generate exciting force. A compaction work road surface is constructed in a discrete element simulation software, the particle attributes, the particle number and the contact parameters are set, different geometric shapes and different sizes of soil and stone particle models are randomly generated, and the accumulation and adhesion of the soil and stone particle models are set to establish a roadbed model; The vibrating roller and the wheels in the multi-body system dynamics simulation software are imported into the discrete element simulation model to establish a discrete element-multiple body dynamics simulation model of the road roller robot; The kinematic model of the road roller robot is: wherein is the forward speed of the vibratory roller, is the speed is the component in the X axis, is the speed is the component in the Y axis, is the heading of the vibratory roller, is the rate of change of the heading angle, is the turning angle of the roller, is the distance from the hinge point to the center of the vibratory roller, is the distance from the hinge point to the center of the rear frame.
9. The path following control system based on discrete element-multiple body dynamics coupling of claim 7, wherein, The system state estimation module obtains the optimal estimation of the current system state as follows: The constructed discrete element-multiple body dynamics coupling model is used for offline simulation of typical working conditions; the actual state data of the road roller robot during the simulation process are collected, and the theoretical state data are calculated at the same time by the constructed kinematic model of the road roller robot, and the deviation sequence of the actual state data and the theoretical state data is calculated; Statistical analysis is performed on the bias sequence to calculate its covariance matrix, which is directly mapped to the process noise covariance matrix of the Kalman filter Q ; The kinematic model of the road roller robot is converted into a state equation as follows: wherein the state quantity is , respectively represent the components of the forward speed of the vibratory roller in the X-axis and Y-axis and the heading of the vibratory roller, and the control quantity is , is a state transition matrix; The disturbance of the road roller robot working on an unstructured road surface is expressed as a process noise of the state space equation: wherein, is a state transition matrix; is a control input matrix; is process noise due to the road robot being affected by the granular road surface, process noise subject to the expectation being 0, the covariance matrix is Q normally distributed, i.e. ; The observation vector of the road roller robot is defined as: wherein is an observed value of the actual value affected by measurement noise, is an output matrix; In the state-space equations, the estimated values are used. Replace actual value And introduce measurement estimation Data fusion is performed using a Kalman filter to convert the estimated values. With measurement estimation After fusion and adjustment, the state quantity estimates after Kalman filtering are obtained: wherein, is a prior state estimate, is a Kalman gain, is an observation vector affected by measurement noise, is an output matrix; The state quantity estimation value after Kalman filtering As a feedback input of model predictive control, the state equation is expressed as ; The state equation is expanded to the second order Taylor series at the target state and the high-order terms are ignored; then, the state equation is subtracted from the target state and discretized to obtain the linear error model of the road roller robot.
10. The path following control system based on discrete element-multiple body dynamics coupling of claim 7, wherein, The roller robot control module acquires an optimal control increment of a current control period, and specifically as follows: The control amount is converted into a control increment, a new state amount is constructed, and a new state equation is constructed; The new state amount is: A new state space expression is obtained as follows: wherein denotes the augmented state vector, denotes k the state error vector at time instant, denotes k the control error vector at time instant, , denotes the augmented system state matrix and control matrix, denotes the identity matrix; Based on the reference trajectory, the current system state, and the control amount, the future state of the system is calculated by using a prediction model, and an optimization problem is solved online by using a rolling optimization strategy. The control amount is adjusted to make the predicted state approach the reference trajectory to reduce the path tracking deviation. The optimal control increment obtained is output as a command in real time, and this process is repeated in each control period. In the process of solving the control parameters of the roller robot by using the model predictive control, the objective function is as follows: wherein, , , is a weight matrix, is a relaxation factor, is a prediction horizon, is a control horizon, is a deviation of the actual output from the reference output, is a control increment, is a minimum value of the input control, is a maximum value of the input control, is a minimum value of the input control increment, is a maximum value of the input control increment.