A time-domain trajectory tracking control method and control system for a mobile robot

Abstract

The application discloses a kind of time-domain trajectory tracking control method and control system of mobile robot, belong to mobile robot trajectory tracking control technical field.The application is by analyzing the influence of predicted time-domain parameter on trajectory tracking performance under different road surface friction coefficient, selects the optimal predicted time-domain corresponding to each friction coefficient, and the corresponding relationship between friction coefficient and optimal predicted time-domain is established using PCHIP piecewise cubic Hermite interpolation method, so that the controller can dynamically adjust the predicted time-domain parameter according to the road adhesion condition;At the same time, the road surface friction coefficient is estimated in real time by introducing the unscented Kalman filter algorithm, and the estimation result is used for online updating of MPC controller prediction time domain, so that the mobile robot can maintain good trajectory tracking accuracy and motion stability under different road conditions.

Description

Technical Field

[0001] This invention relates to the field of mobile robot trajectory tracking and control technology, and more specifically, to a time-domain trajectory tracking and control method and control system for a mobile robot. Background Technology

[0002] With the development of intelligent robots and unmanned operation technologies, mobile robots are increasingly widely used in road construction, maintenance, road marking, painting, and inspection. During road operations, robots need to move autonomously along a preset trajectory while maintaining high trajectory tracking accuracy and motion stability. Four-wheel drive, four-steering mobile robots have independent drive and steering capabilities for all four wheels, offering high maneuverability, a small turning radius, and flexible movement modes, making them suitable for autonomous operations in complex road environments.

[0003] However, road surface adhesion conditions vary significantly under different road environments, resulting in large variations in the friction coefficient. This can easily lead to increased trajectory tracking errors, control input fluctuations, and decreased motion stability. While existing PID control methods are simple in structure, they are not adaptable enough to complex trajectories and variations in road surface friction coefficients. Although traditional MPC control can handle state, input, and actuator constraints, it typically uses a fixed prediction time domain, making it difficult to balance stability under low-adhesion road surfaces with rapid response under high-adhesion road surfaces, thus affecting the trajectory tracking control performance.

[0004] For example, patent CN119336029A discloses an unmanned vehicle trajectory tracking control method based on adaptive time-domain model predictive control. This method constructs a model predictive trajectory tracking controller to adjust the prediction time domain and control time domain of the tested vehicle in real time under different operating conditions. Although this method can improve the trajectory tracking adaptability of unmanned vehicles under different operating conditions, it ignores the impact of changes in the road surface friction coefficient.

[0005] For example, patent CN114368369A discloses an integrated control method for a chassis braking system that adapts to the road surface friction coefficient. This method adjusts the desired state value in the controller based on the road surface friction coefficient and adjusts the weighting coefficient of the objective function based on the real-time vehicle state, thereby maintaining the integrated control performance of the vehicle chassis when the road surface adhesion condition changes. Although this method considers the impact of changes in the road surface friction coefficient on vehicle control performance, it mainly adapts to changes in the friction coefficient by adjusting the desired state value and the weighting coefficient of the objective function, and the control accuracy still needs further improvement. Summary of the Invention

[0006] The present invention aims to provide a time-domain trajectory tracking control method and control system for mobile robots, thereby solving the technical problem that existing control methods are unable to simultaneously achieve both trajectory tracking accuracy and motion stability under different road surface adhesion conditions.

[0007] To achieve the above objectives, the technical solution provided by the present invention is as follows: This invention provides a time-domain trajectory tracking control method for a mobile robot, comprising the following steps: Construct a predictive model for time-domain trajectory tracking control of mobile robots; Based on the prediction model, an objective function and constraints are designed to construct the MPC trajectory tracking controller for the mobile robot. Construct an optimal predictive time-domain control law based on the road surface friction coefficient; By combining the MPC trajectory tracking controller with the optimal prediction time-domain control law, using the road friction coefficient as the adaptive adjustment variable and the prediction time-domain parameters as the online adjustment object, a friction coefficient adaptive prediction time-domain MPC controller for a mobile robot is constructed. The real-time road friction coefficient of the mobile robot during trajectory tracking is obtained and input into the friction coefficient adaptive prediction time domain MPC controller to dynamically adjust the prediction time domain parameters of the MPC controller. The optimal control quantity of the current control cycle is solved by rolling optimization, and the optimal control quantity is converted into the driving torque command and steering angle command of the mobile robot, thereby controlling the mobile robot to track the reference trajectory.

[0008] Furthermore, the construction of the predictive model for time-domain trajectory tracking control of the mobile robot includes: A dynamic model of the mobile robot is established based on its motion state, tire force characteristics, and drive steering mechanism characteristics. The dynamic model of the mobile robot is transformed into a continuous-time nonlinear state-space model and discretized to obtain a predictive model for trajectory tracking control. The dynamic model of the mobile robot includes a tire dynamic model and a vehicle dynamic model, wherein the vehicle dynamic model includes a vehicle translational dynamics model, a vehicle rotational dynamics model, and a vehicle steering dynamics model.

[0009] Furthermore, the continuous-time nonlinear state-space model is discretized using the forward Euler method, resulting in the following prediction model for trajectory tracking control: ; in, x k For the first k System state quantities at each sampling time, u k For the first k The control input at each sampling time The coefficient of friction of the road surface. This is the discretized state transition function; y k For the first k The output at each sampling time This is the output function; the system state variables and control input variables are defined as follows: ; ; In the above formula, X and Y These represent the longitudinal and lateral positions of the mobile robot in the inertial coordinate system, respectively. i For heading angle, v x and v y These are the longitudinal velocity and lateral velocity in the vehicle's coordinate system, respectively. oh The yaw rate is angular velocity. oh i，i=1,2,3,4 For the first i The angular velocity of each wheel d i，i=1,2,3,4 For the first i The actual steering angle of each wheel T i，i=1,2,3,4 For the first i The driving torque of each wheel d i,cmd，i=1,2,3,4 For the first i The target steering angle for each wheel; The model output is selected as the robot's position, attitude, and motion state that need to be constrained and tracked in trajectory tracking control, namely: .

[0010] Furthermore, the objective function includes a trajectory tracking error term, a control input term, and a control input increment term, expressed as follows: ; in, Q This is the trajectory tracking error weight matrix. R u To control the input weight matrix, R Δu To control the input increment weight matrix; Y k and Y ref, k These are the model prediction output sequence and the reference trajectory sequence, respectively. U k and △ U k These are the control input sequence and the control input increment sequence, respectively. The constraints include control input constraints, control input increment constraints, output constraints, and tire adhesion constraints. The control input constraints, control input increment constraints, and output constraints are used to constrain the upper and lower limits of the control input, control input increment, and output, respectively. The tire adhesion constraint is expressed as follows: ; In the above formula, F long,i and F lat,i These represent the longitudinal and lateral tire forces in the tire coordinate system, respectively, calculated from the tire dynamics model. F z,i For the first i Vertical load on each wheel m The coefficient of friction of the road surface; The MPC trajectory tracking control problem of the mobile robot is transformed into: solving for the objective function that satisfies the prediction model and the above constraints. J k The control input sequence that achieves the minimum value.

[0011] After solving the above optimization problem, the optimal control input sequence is obtained. According to the MPC rolling optimization principle, only the first control quantity in the optimal control input sequence is applied to the mobile robot. This control quantity corresponds to the driving torque command and the target steering angle command of the four wheels. After executing the control variable, the robot proceeds to the next sampling time and repeats the above prediction and optimization process.

[0012] Furthermore, the construction of the optimal predictive time-domain control law based on the road surface friction coefficient includes: Under different road surface friction coefficients, different prediction time domain parameters were set to conduct trajectory tracking simulations or experiments to obtain trajectory tracking performance data of mobile robots under different friction coefficients and different prediction time domain combinations. The optimal prediction time-domain parameters corresponding to the friction coefficients of each road surface are determined based on the trajectory tracking performance data, and the correspondence between the road surface friction coefficients and the optimal prediction time-domain parameters is fitted to obtain the optimal prediction time-domain control law based on the road surface friction coefficients.

[0013] Furthermore, the step of determining the optimal prediction time-domain parameters corresponding to the friction coefficients of each road surface based on trajectory tracking performance data, and fitting the correspondence between the road surface friction coefficients and the optimal prediction time-domain parameters to obtain the optimal prediction time-domain control law based on the road surface friction coefficients, specifically includes: Based on the actual output of the mobile robot obtained from simulation or experiment y k and reference output yref, k Calculate the trajectory tracking error; A comprehensive evaluation index is constructed based on trajectory tracking error, motion stability, and control input smoothness: ; in, e y The lateral trajectory error is used to evaluate trajectory tracking accuracy. e θ For heading angle error, e v For velocity error, both are used to evaluate the motion stability of a mobile robot; Δ u To control the input increment, used to evaluate the smoothness of the control input; w 1, w 2, w 3, w 4 represents the evaluation weighting coefficient; For the same road surface friction coefficient, select the comprehensive evaluation index. J eva The smallest prediction time domain is taken as the optimal prediction time domain under this friction coefficient; The above process is repeated for multiple road surface friction coefficients to obtain a set of discrete correspondence data between the road surface friction coefficient and the optimal prediction time domain; The PCHIP piecewise cubic Hermite interpolation method is used to fit the above discrete corresponding data, thereby obtaining the optimal predictive time-domain control law based on the road friction coefficient: ; in, N p For prediction in the time domain; It is the interpolation function obtained by fitting the road surface friction coefficient with the optimal prediction time-domain discrete data.

[0014] Furthermore, when solving for the optimal prediction time domain based on the road surface friction coefficient, further... f PCHIP ( m The output of ) is rounded and limited: ; in, N p, min and N p, max These represent the minimum and maximum values ​​allowed in the prediction time domain, respectively.

[0015] Furthermore, the method of obtaining the real-time road friction coefficient when the mobile robot is performing trajectory tracking operation specifically involves: establishing a real-time road friction coefficient estimator based on the unscented Kalman filter algorithm. The real-time road friction coefficient estimator uses the road friction coefficient as the state variable to be estimated and estimates the current road friction coefficient in real time based on the state feedback information of the mobile robot and the tire force information. The status feedback information includes at least one of the mobile robot's position, heading angle, longitudinal velocity, lateral velocity, and yaw rate, and the tire force information includes at least one of the tire longitudinal force, tire lateral force, and tire vertical load.

[0016] Furthermore, the real-time road friction coefficient estimator uses the road friction coefficients of the four wheels as the state variables to be estimated, and performs a weighted average fusion of the estimated friction coefficients of the four wheels to obtain the road friction coefficients used for predicting time-domain updates in the current control cycle.

[0017] Specifically, when the mobile robot performs trajectory tracking, it acquires real-time robot state feedback information and tire force information, and obtains the current road friction coefficient estimate through a real-time road friction coefficient estimator. This estimate is then input into the friction coefficient adaptive prediction time-domain MPC controller, which updates the MPC controller's prediction time-domain parameters online based on the optimal prediction time-domain control law. Simultaneously, the tire adhesion constraints are updated based on the current road friction coefficient estimate, enabling the MPC controller to solve for the optimal control quantity that satisfies the constraints under the current road adhesion conditions. The optimal control quantity includes the driving torque command for all four wheels and the target steering angle command for all four wheels. After executing the optimal control quantity, the mobile robot enters the next control cycle and repeats the road friction coefficient estimation, prediction time-domain update, adhesion constraint update, and MPC rolling optimization solution process to achieve adaptive trajectory tracking control under different road adhesion conditions.

[0018] The present invention also provides a time-domain trajectory tracking control system for a mobile robot, comprising: The prediction model building module is configured to: build a prediction model for time-domain trajectory tracking control of a mobile robot; The trajectory tracking controller building module is configured to: design objective functions and constraints based on the prediction model, and build the MPC trajectory tracking controller for the mobile robot; The optimal prediction time-domain control law construction module is configured to: construct the optimal prediction time-domain control law based on the road surface friction coefficient; The friction coefficient adaptive prediction time domain MPC controller construction module is configured to: combine the MPC trajectory tracking controller with the optimal prediction time domain control law, use the road friction coefficient as the adaptive adjustment variable, and use the prediction time domain parameters as the online adjustment object to construct the friction coefficient adaptive prediction time domain MPC controller for the mobile robot. The trajectory tracking control module of the mobile robot is configured to: acquire the real-time road friction coefficient when the mobile robot is performing trajectory tracking operations and input it into the friction coefficient adaptive prediction time domain MPC controller to dynamically adjust the prediction time domain parameters of the MPC controller, and solve the optimal control quantity of the current control cycle through rolling optimization, and convert the optimal control quantity into the driving torque command and steering angle command of the mobile robot, thereby controlling the mobile robot to track the reference trajectory. Compared with the prior art, the technical solution provided by this invention has the following advantages: (1) Based on the ordinary MPC trajectory tracking controller, the present invention introduces an adaptive prediction time domain adjustment mechanism for road friction coefficient, which enables the MPC controller to dynamically adjust the prediction time domain parameters according to the current road adhesion conditions, thereby avoiding the problem of insufficient adaptability of fixed prediction time domain under different friction coefficient road surfaces.

[0019] (2) This invention analyzes the trajectory tracking performance under different road surface friction coefficients and different prediction time domain combinations, determines the optimal prediction time domain parameters corresponding to each friction coefficient, and establishes the optimal prediction time domain control law through PCHIP interpolation, so that the mobile robot can adjust the control strategy in real time when the road surface adhesion conditions change, and avoids abrupt changes in the prediction time domain within adjacent friction coefficient intervals, making the prediction time domain more continuous and smooth with the change of road surface friction coefficient, thus improving the trajectory tracking accuracy, motion stability and adaptability to complex road surfaces.

[0020] (3) The present invention further incorporates the road friction coefficient directly into the constraint conditions so that the driving torque and steering angle commands obtained by the controller meet the current road adhesion capability, thereby reducing the risk of tire slippage, excessive lateral deviation or excessive control on low adhesion road surfaces. Attached Figure Description

[0021] Figure 1 This is a flowchart illustrating the time-domain trajectory tracking and control method for a mobile robot according to an embodiment of the present invention. Figure 2 This is a schematic diagram of the overall architecture of the friction coefficient adaptive prediction time-domain MPC controller in an embodiment of the present invention; Figure 3 This is a schematic diagram showing the correspondence between the road surface friction coefficient and the optimal predicted time-domain parameters in an embodiment of the present invention, as well as the PCHIP fitting. Figure 4 This is a schematic diagram comparing the trajectory tracking performance of the control method of this invention with that of ordinary MPC control method and PID control method. Detailed Implementation

[0022] To further understand the present invention, it will now be described in detail with reference to the accompanying drawings and embodiments. However, it should be understood that these descriptions are exemplary only and are not intended to limit the scope of this disclosure. In the following detailed description, numerous specific details are set forth to provide a thorough understanding of the embodiments of this disclosure for ease of explanation. However, it will be apparent that one or more embodiments may be practiced without these specific details. Furthermore, descriptions of well-known structures and techniques are omitted in the following description to avoid unnecessarily obscuring the concepts of this disclosure.

[0023] The terminology used herein is for the purpose of describing particular embodiments only and is not intended to limit this disclosure. The terms “comprising,” “including,” etc., as used herein indicate the presence of the stated features, steps, operations, and / or components, but do not exclude the presence or addition of one or more other features, steps, operations, or components.

[0024] All terms used herein (including technical and scientific terms) have the meanings commonly understood by those skilled in the art, unless otherwise defined. It should be noted that the terms used herein are to be interpreted in a manner consistent with the context of this specification, and not in an idealized or overly rigid way.

[0025] This invention provides a temporal trajectory tracking control method for a mobile robot. This method is applicable to trajectory tracking control of mobile robots in various scenarios such as road marking layout, road marking painting, road inspection, and road maintenance, and can be executed by the mobile robot control system. Combined with... Figure 1 As shown, the following steps are taken to track and control the time-domain trajectory of a four-wheel drive, four-rudder mobile robot: Step 1: Construct a predictive model for time-domain trajectory tracking control of mobile robots; S11. Based on the motion state of the mobile robot, the force characteristics of the tires, and the characteristics of the drive steering actuator, establish a dynamic model of the mobile robot. Furthermore, the dynamic model of the mobile robot includes a tire dynamic model and a vehicle dynamic model. Wherein: (1) Tire dynamics model The expression for the tire dynamics model in the tire coordinate system is: ; In the above formula, The longitudinal tire force in the tire coordinate system, in N. This represents the lateral tire force in the tire coordinate system. The tire rolling resistance in the tire coordinate system, in N. f roll This is the tire rolling resistance coefficient. The coefficient of friction of the road surface. For slip ratio, Side slip angle, in rad. , , , These are the parameters for fitting longitudinal tire forces. , , , These are the fitting parameters for lateral tire forces. The negative sign indicates that the direction of the lateral force is opposite to the direction of the slip angle. Vertical load per wheel, in N. v B,i The longitudinal speed of the wheel, in m / s. v th The low-speed threshold is expressed in m / s, and the subscripts in the above physical quantities are... i Indicates the first i The physical quantities corresponding to each wheel.

[0026] The tire dynamics model is transformed from the tire coordinate system to the vehicle coordinate system, expressed as follows: ; in, The longitudinal component of the force exerted by the wheel on the vehicle body in the vehicle body coordinate system, in N. The lateral force exerted by the wheel on the vehicle body in the vehicle body coordinate system, in N. , These represent the tire rolling resistance in the vehicle body coordinate system. x , y Components in direction, in N. The turning angle is expressed in rad.

[0027] (2) Vehicle dynamics model In the vehicle coordinate system, the overall vehicle dynamics model is divided into the overall vehicle translational dynamics model, the overall vehicle rotational dynamics model, and the overall vehicle steering dynamics model (or steering actuator dynamics model). The overall vehicle translational dynamics model is as follows: ; in, m The unit is the total mass of the mobile robot, expressed in kg.

[0028] The overall vehicle rotational dynamics model is as follows: ; in, For vehicles to bypass Moment of inertia of the shaft, in kg·m². x B,i , y B,i These are the longitudinal and lateral position coordinates of the wheel relative to the robot's center of mass or the origin of the vehicle's coordinate system, respectively.

[0029] The vehicle steering dynamics model is as follows: ; in, This is the time constant of the steering system.

[0030] Furthermore, the system state variables are defined as follows: ; Define the control input as: ; in, X and Y These represent the longitudinal and lateral positions of the mobile robot in the inertial coordinate system, respectively. i Heading angle, in rad. v x and v y These represent the longitudinal and lateral velocities in the vehicle's coordinate system, in m / s. oh The yaw rate is expressed in rad / s. oh i For the first i The angular velocity of each wheel, in rad / s. d i For the first i The actual steering angle of each wheel, in rad. T i For the first i The driving torque of each wheel, in N·m. d i,cmd For the first i The target steering angle for each wheel, in m / s.

[0031] Furthermore, the robot position and heading angle in the system state variables can be obtained by integrating the vehicle speed and yaw rate, and their pose kinematic relationship is as follows: .

[0032] Furthermore, the four wheel angular velocities in the system state variables oh i Determined by the wheel rotation dynamics model, the first i The rotational dynamics equations for each wheel are:

[0033] in, I w The moment of inertia of the wheel, expressed in kg·m². R The radius of the wheel, in meters. b w This is the wheel rotation damping coefficient.

[0034] Therefore, the driving torque in the control input can be understood. T i By changing the wheel angular velocity oh i This further affects the slip ratio. k i This affects the aforementioned longitudinal force of the tire. F long,i .

[0035] S12. The dynamic model of the mobile robot is transformed into a state-space equation form and discretized to obtain a prediction model for trajectory tracking control.

[0036] From the tire dynamics model, coordinate transformation relationship, vehicle dynamics model, vehicle steering dynamics model, and steering actuator dynamics model mentioned above, it can be seen that the road friction coefficient... m Firstly, it involves the longitudinal force of the tire. F long,i and lateral force F lat,i The calculation; subsequently, F long,i and F lat,i By wheel steering angle d i Converted to longitudinal component in vehicle body coordinate system F B,x,i and lateral component force F B,y,i ;at last, F B,x,i and F B,y,i Enter the vehicle's translational dynamics equations and rotational dynamics equations for calculation. v x , v y and oh The changes in kinetic energy. Therefore, the transmission relationship between the aforementioned dynamic models can be expressed as: ; By unifying and organizing the above models, we can obtain the continuous-time nonlinear state-space model of the (four-wheel drive, four-rudder) mobile robot: ; Its output is selected as the robot's position, attitude, and motion state that need to be constrained and tracked in trajectory tracking control, that is: ; in, It is determined by the aforementioned posture kinematics model, tire dynamics model, tire force coordinate transformation model, vehicle translational motion mechanical model, vehicle rotational dynamics model, wheel rotational dynamics model, and steering actuator dynamics model. This is the output function.

[0037] Furthermore, to enable the mobile robot's dynamic model to be used for prediction calculations in the MPC trajectory tracking controller, the aforementioned continuous-time nonlinear state-space model is discretized. Optionally, the discretization method in this embodiment is as follows: Let the sampling period be... T s Discretization using the forward Euler method yields: ; Recorded as: ; The corresponding output is: ; in, x k For the first k System state quantities at each sampling time, u k For the first k The control input at each sampling time Let be the discretized state transition function. Therefore, the prediction model for MPC trajectory tracking control is obtained as follows: .

[0038] Furthermore, in the k At each sampling time, the MPC controller determines the current state. x k Control input u k and road surface friction coefficient m The future state is then recursively predicted using the aforementioned prediction model. Let the prediction time domain be... N p Control time domain is N c Then the future number j The predicted state of the step can be represented as: ; The corresponding prediction output is: ; Stacking the output values ​​in the predicted time domain yields the predicted output sequence: ; The pre-defined reference trajectory sequence is as follows: ; The incremental sequence of the control input is: ; in: .

[0039] Step 2: Based on the prediction model in Step 1, design the objective function and constraints, and construct the MPC trajectory tracking controller for the mobile robot; Furthermore, the objective function includes a trajectory tracking error term, a control input term, and a control input increment term, expressed as: ; in, Q This is the trajectory tracking error weight matrix. R u To control the input weight matrix, R Δu To control the input increment weight matrix, the first term in the objective function above utilizes the aforementioned predicted output sequence. Y k and reference trajectory sequence Y ref,k The first constraint is to calculate the trajectory tracking deviation; the second constraint is to calculate the magnitude of the drive torque and steering angle commands obtained by MPC; the third constraint is to determine the change in control input between adjacent sampling times to ensure smooth changes in drive torque and steering angle commands.

[0040] Furthermore, the constraints of the MPC trajectory tracking controller are jointly determined by the robot dynamics model and the physical limitations of the actuator. Due to the control input... u The driving torque and target steering angle of the four wheels are defined, therefore the control input constraints are: ; in, u min and u max These are the lower and upper limits for controlling the input, respectively.

[0041] Due to Δ U k This represents the control input increment sequence. To avoid abrupt changes in drive torque and steering angle commands within adjacent control cycles, the control input increment constraint is set as follows: .

[0042] Where, Δ u min Δ u max These are the lower and upper limits for controlling the input increment, respectively.

[0043] Due to output y Selected as X , Y , i , v x , v y , oh Among them, velocity and yaw rate are directly related to the robot's motion stability, therefore the output constraints are set as follows: .

[0044] in, y min , y max These are the lower and upper limits of the output, respectively.

[0045] Furthermore, the aforementioned tire dynamics model yielded... F long,i and F lat,i It is also used to construct tire adhesion constraints. Since the longitudinal and lateral forces of the tire cannot exceed the adhesion limit allowed by the current road surface friction coefficient, therefore for the first... i There are 1 wheel, and:

[0046] in, F long,i and F lat,i The results have been calculated using the aforementioned tire dynamics model. F z,i For the first i Vertical load on each wheel m Let be the road surface friction coefficient. This constraint indicates that the smaller the current road surface friction coefficient, the smaller the net force that the tires can provide, and the more limited the range of control inputs that the MPC controller can select during the solution process. By directly incorporating the road surface friction coefficient into the constraint conditions through this formula, the driving torque and steering angle commands obtained by the controller can meet the current road surface adhesion capabilities. Therefore, it helps to reduce the risk of tire slippage, excessive lateral deviation, or overly aggressive control on low-adhesion roads.

[0047] Therefore, at the k-th sampling time, the MPC trajectory tracking control problem of the mobile robot can be expressed as:

[0048] in, Let be the optimal control input sequence obtained at the current sampling time, representing the sequence that, under the conditions of satisfying the prediction model, control input constraints, control input increment constraints, output constraints, and tire adhesion constraints, makes the objective function ... J k The control input sequence that achieves the minimum value, with the superscript * indicating the optimal value obtained through optimization.

[0049] That is, the control expression of the MPC trajectory tracking controller of the mobile robot is as follows: ; in, j =0,1, , N p 1, i =1, 2, 3, 4. This formula integrates prediction model constraints, control input constraints, control input increment constraints, output constraints, and tire adhesion constraints into the MPC optimization problem. This ensures that trajectory tracking control not only considers error minimization but also simultaneously takes into account actuator physical limitations, control smoothness, motion stability, and road adhesion limitations. Compared to traditional MPC that only considers input and output constraints, this constraint set improves the feasibility and safety of four-wheel drive, four-rudder mobile robots under complex road conditions.

[0050] After solving the above optimization problem, the optimal control input sequence is obtained: .

[0051] According to the MPC rolling optimization principle, only the first control variable in the optimal control input sequence is applied to the mobile robot, that is: ; This control input corresponds to the drive torque command and the target steering angle command for the four wheels. After executing this control input, the robot enters the next sampling time, reacquires the state feedback information, and repeats the above prediction and optimization process.

[0052] Step 3: In order to enable the fixed prediction time domain MPC controller to adapt to different road surface adhesion conditions, different prediction time domain parameters are set under different road surface friction coefficients to conduct trajectory tracking simulations or experiments, so as to obtain mobile robot trajectory tracking performance data under different friction coefficients and different prediction time domain combinations.

[0053] Specifically, given a certain road surface friction coefficient m s At that time, first ms Substituting these values ​​into the aforementioned tire dynamics model and tire adhesion constraints, and selecting different prediction time domains respectively... N p MPC trajectory tracking control simulation was performed to obtain different ( m s , N p The trajectory tracking results under the combination of [data missing]. The trajectory tracking performance data includes at least one of the following: trajectory tracking error, heading angle error, velocity tracking error, motion stability evaluation data, and control input variation.

[0054] Step 4: Determine the optimal prediction time-domain parameters corresponding to each road surface friction coefficient based on the trajectory tracking performance data, and fit the correspondence between the road surface friction coefficient and the optimal prediction time-domain parameters to obtain the optimal prediction time-domain control law based on the road surface friction coefficient.

[0055] The control effectiveness of different predicted time-domain parameters under the same road surface friction coefficient is evaluated based on trajectory tracking performance data to determine the optimal predicted time-domain parameters for that road surface friction coefficient. The specific steps are as follows: First, based on the actual output of the robot obtained from the simulation or experiment in step three. y k and reference output y ref, k Calculate the trajectory tracking error: ; Next, a comprehensive evaluation index is constructed based on trajectory tracking error, motion stability, and control input smoothness: ; in, e y This refers to the lateral trajectory error. e θ For heading angle error, e v For the speed error, Δ u To control the input increment, w 1, w 2, w 3, w 4 represents the evaluation weighting coefficient.

[0056] For the same road surface friction coefficient m s Select comprehensive evaluation indicators J eva The smallest prediction time domain is taken as the optimal prediction time domain for this friction coefficient: ; The optimal prediction time domain parameter for the road surface condition is obtained by selecting the prediction time domain with the smallest comprehensive evaluation index from multiple candidate prediction time domains. Compared with the method of selecting a fixed prediction time domain by human experience, this formula transforms the prediction time domain selection process into a quantifiable and repeatable optimization process, thereby improving the objectivity of the prediction time domain parameter determination.

[0057] By repeating the above process for multiple road surface friction coefficients, a set of discrete corresponding data is obtained: ; in, m 1~ m n For the selected road surface friction coefficient samples, n The formula represents the sample size. It is used to organize the discrete sample relationships between different road surface friction coefficients and their corresponding optimal prediction time domains, providing a data foundation for subsequent interpolation and fitting. Compared to setting a fixed prediction time domain only for a few friction coefficient conditions, this formula can form a parameter database of friction coefficient-prediction time domain, providing a basis for adaptive adjustment under continuous changes in road surface adhesion.

[0058] Furthermore, in this embodiment of the invention, the PCHIP piecewise cubic Hermite interpolation method is preferably used to fit the above discrete corresponding data, thereby obtaining the optimal prediction time-domain control law based on the road friction coefficient (e.g., Figure 3 (as shown) ; in, This is the interpolation function obtained by fitting the road surface friction coefficient to the discrete data of the optimal prediction time domain. By establishing a continuous mapping relationship between the road surface friction coefficient and the optimal prediction time domain through PCHIP interpolation, compared to the lookup table method or piecewise constant adjustment method, this formula can avoid abrupt changes in the prediction time domain within adjacent friction coefficient intervals, making the prediction time domain more continuous and smooth as the road surface friction coefficient changes, thereby improving the stability of the controller during online switching.

[0059] Since the prediction time domain is integer, further adjustments are needed during actual control. f PCHIP ( m The output of ) is rounded and limited: ; in, N p, min and N p, max These represent the minimum and maximum values ​​allowed in the prediction time domain, respectively. Therefore, the aforementioned fixed prediction time domain... N pThe time domain is replaced with an adaptive prediction that varies with the road surface friction coefficient. Further rounding and amplitude limiting effectively ensure that the predicted time domain parameters meet the integer requirements of MPC optimization while remaining within the allowable range for computational real-time performance and control stability, thus improving the method's engineering feasibility.

[0060] Step 5: Combine the MPC trajectory tracking controller with the optimal prediction time-domain control law, using the road friction coefficient as the adaptive adjustment variable and the prediction time-domain parameters as the online adjustment object, to construct a friction coefficient adaptive prediction time-domain MPC controller for the mobile robot, as follows. Figure 2 As shown, this enables the MPC controller to select the appropriate prediction time-domain parameters based on the current road surface friction coefficient.

[0061] Specifically, in the aforementioned prediction recursive formula and objective function, the prediction time domain... N p Instead of taking a fixed value, the controller is determined by the optimal prediction time-domain control law corresponding to the current road surface friction coefficient. Therefore, when the road surface friction coefficient is small, the controller selects a longer prediction time domain, thereby improving the ability to predict future motion states and changes in adhesion constraints; when the road surface friction coefficient is large, the controller selects a shorter prediction time domain, thereby improving the local trajectory tracking response speed and computational real-time performance.

[0062] Step Six: When the mobile robot is performing trajectory tracking, the road friction coefficient is input into the friction coefficient adaptive prediction time domain MPC controller. The prediction time domain parameters of the MPC controller are dynamically adjusted, and the optimal control quantity of the current control cycle is solved by rolling optimization. The optimal control quantity is converted into the driving torque command and steering angle command of the mobile robot, thereby controlling the mobile robot to track the reference trajectory.

[0063] Furthermore, to enable the aforementioned friction coefficient adaptive prediction time-domain MPC controller to operate online in real time and improve control accuracy, this embodiment of the invention establishes a real-time road friction coefficient estimator. Based on the mobile robot's state feedback information and tire force information, the current road friction coefficient is estimated in real time. The estimated real-time road friction coefficient can then be input into the friction coefficient adaptive prediction time-domain MPC controller to dynamically adjust the MPC controller's prediction time-domain parameters. The state feedback information includes at least one of robot position, heading angle, longitudinal velocity, lateral velocity, and yaw rate; the tire force information includes at least one of tire longitudinal force, tire lateral force, and tire vertical load.

[0064] Specifically, in a preferred embodiment of the present invention, a real-time road friction coefficient estimator is established based on an unscented Kalman filter algorithm. According to the aforementioned vehicle translational motion dynamics model and vehicle rotational dynamics model, the longitudinal acceleration, lateral acceleration, and yaw rate acceleration are all determined by… F B,x,i and F B,y,i Decision, and F B,x,i and F B,y,i It also includes the road surface friction coefficient m Tire force F long,i and F lat,i The calculation yielded the result. Therefore, the road surface friction coefficient can be used as the state variable to be estimated.

[0065] Define the state variables of the friction coefficient estimator as follows: ; in, m i For the first i The road friction coefficient corresponding to each wheel. Considering the continuous change of the road friction coefficient between adjacent sampling times, the state equation can be expressed as: ; in, w k This is process noise.

[0066] Furthermore, based on the aforementioned vehicle dynamics equations, the following measurements are constructed:

[0067] Combining the vehicle's translational dynamics and rotational dynamics models, we have: ; in, v k To measure noise. Because F B,x,i and F B,y,i Based on the aforementioned tire dynamics model and considering its relation to the road surface friction coefficient, the above measurement equation can be written as: ; This leads to the state equation and measurement equation used for unscented Kalman filtering: .

[0068] Furthermore, the unscented Kalman filter algorithm generates Sigma points based on the friction coefficient estimate from the previous moment, and substitutes these Sigma points into the aforementioned state equation and measurement equation to complete state prediction, measurement prediction, Kalman gain calculation, and state update, thereby obtaining the estimated road surface friction coefficient for the current moment. ; in, For the first k The estimated value of the friction coefficient at any given time. For the first i Estimated coefficient of friction for each wheel.

[0069] By fusing the estimated friction coefficients for the four wheels, the road friction coefficient used for predicting time-domain updates in the current control cycle is obtained. : .

[0070] Furthermore, during trajectory tracking, the mobile robot acquires real-time status feedback information and tire force information, and obtains an estimated value of the current road surface friction coefficient through the real-time road surface friction coefficient estimator. The estimated value of the current road surface friction coefficient The input is fed to the friction coefficient adaptive prediction time-domain MPC controller, and the prediction time-domain parameters of the MPC controller are updated online according to the optimal prediction time-domain control law, thereby obtaining the adaptive prediction time-domain for the current control cycle: ; Then, N p,k Substitute these values ​​into the MPC prediction model and objective function to update the prediction output sequence. Y k Reference trajectory sequence Y ref,k And optimize the dimensions of the problem; at the same time, Substitute the results into the tire dynamics model and tire adhesion constraints to update the tire force calculation results and adhesion constraint range under the current road conditions.

[0071] At this time, the MPC controller determines the current state. x k Reference trajectory Y ref,k Adaptive prediction time domain N p,k Estimated value of road surface friction coefficient The rolling optimization problem is then resolved to obtain the optimal control quantities for the current control cycle. These optimal control quantities include the drive torque commands for the four wheels and the target steering angle commands for the four wheels, as detailed below: ; Finally, the optimal control variables mentioned above... As the driving torque command for all four wheels, The target steering angle command is used as the steering angle command for the four wheels and is applied to the four-wheel drive, four-rudder mobile robot.

[0072] After executing control commands, the robot enters the next sampling cycle, re-collects state feedback information and tire force information, re-estimates the road friction coefficient, and repeats the prediction time-domain update and MPC rolling optimization process to achieve adaptive trajectory tracking control under different road surface adhesion conditions.

[0073] Through the above implementation methods, the aforementioned tire dynamics model, vehicle body coordinate system force transformation model, vehicle dynamics model, MPC prediction model, optimal prediction time-domain control law, and road friction coefficient real-time estimator form a progressive closed-loop control relationship. The road friction coefficient first affects the tire force and vehicle dynamic response, then the current road friction coefficient is estimated in real time through UKF, which updates the prediction time domain and adhesion constraints of MPC, and finally outputs drive torque and steering angle commands to realize adaptive trajectory tracking control of the four-wheel drive, four-rudder mobile robot under different road adhesion conditions.

[0074] The trajectory tracking performance of the control method of this invention is compared with that of ordinary PC control methods and PID control methods. The results are as follows: Figure 4 As shown in the figure, all three control methods can track the reference trajectory. However, the PID control method exhibits significant tracking lag and lateral deviation during trajectory curvature changes or transition phases. While the conventional MPC control method reduces tracking deviation compared to the PID method, its fixed prediction time domain makes it difficult to simultaneously achieve both tracking accuracy and motion stability when road surface adhesion conditions change. The method of this invention estimates the road friction coefficient in real time and adaptively adjusts the prediction time domain parameters based on the friction coefficient. Simultaneously, it combines tire adhesion constraints with rolling optimization to make the actual trajectory of the mobile robot closer to the reference trajectory, resulting in a smoother trajectory transition. Therefore, compared to the conventional MPC and PID control methods, the method of this invention improves trajectory tracking accuracy and motion stability under different road surface adhesion conditions.

Claims

1. A time-domain trajectory tracking control method for a mobile robot, characterized in that, Includes the following steps: Construct a predictive model for time-domain trajectory tracking control of mobile robots; Based on the prediction model, an objective function and constraints are designed to construct the MPC trajectory tracking controller for the mobile robot. Construct an optimal predictive time-domain control law based on the road surface friction coefficient; By combining the MPC trajectory tracking controller with the optimal prediction time-domain control law, using the road friction coefficient as the adaptive adjustment variable and the prediction time-domain parameters as the online adjustment object, a friction coefficient adaptive prediction time-domain MPC controller for a mobile robot is constructed. The real-time road friction coefficient of the mobile robot during trajectory tracking is obtained and input into the friction coefficient adaptive prediction time domain MPC controller to dynamically adjust the prediction time domain parameters of the MPC controller. The optimal control quantity of the current control cycle is solved by rolling optimization, and the optimal control quantity is converted into the driving torque command and steering angle command of the mobile robot, thereby controlling the mobile robot to track the reference trajectory.

2. The time-domain trajectory tracking control method for a mobile robot according to claim 1, characterized in that, The construction of the predictive model for time-domain trajectory tracking control of mobile robots includes: A dynamic model of the mobile robot is established based on its motion state, tire force characteristics, and drive steering mechanism characteristics. The dynamic model of the mobile robot is transformed into a continuous-time nonlinear state-space model and discretized to obtain a predictive model for trajectory tracking control. The dynamic model of the mobile robot includes a tire dynamic model and a vehicle dynamic model, wherein the vehicle dynamic model includes a vehicle translational dynamics model, a vehicle rotational dynamics model, and a vehicle steering dynamics model.

3. The time-domain trajectory tracking control method for a mobile robot according to claim 2, characterized in that, The continuous-time nonlinear state-space model is discretized using the forward Euler method, and the resulting prediction model for trajectory tracking control is as follows: ； in, x k For the first k System state quantities at each sampling time, u k For the first k The control input at each sampling time The coefficient of friction of the road surface. This is the discretized state transition function; y k For the first k The output at each sampling time This is the output function; the system state variables and control input variables are defined as follows: ； ； In the above formula, X and Y These represent the longitudinal and lateral positions of the mobile robot in the inertial coordinate system, respectively. θ For heading angle, v x and v y These are the longitudinal velocity and lateral velocity in the vehicle's coordinate system, respectively. ω The yaw rate is angular velocity. ω i，i=1,2,3,4 For the first i The angular velocity of each wheel δ i，i=1,2,3,4 For the first i The actual steering angle of each wheel T i，i=1,2,3,4 For the first i The driving torque of each wheel δ i,cmd，i=1,2,3,4 For the first i The target steering angle for each wheel; The model output is selected as the robot's position, attitude, and motion state that need to be constrained and tracked in trajectory tracking control, namely: 。 4. The time-domain trajectory tracking control method for a mobile robot according to any one of claims 1-3, characterized in that, The objective function includes a trajectory tracking error term, a control input term, and a control input increment term, which are expressed as follows: ； in, Q This is the trajectory tracking error weight matrix. R u To control the input weight matrix, R Δu To control the input increment weight matrix; Y k and Y ref, k These are the model prediction output sequence and the reference trajectory sequence, respectively. U k and △ U k These are the control input sequence and the control input increment sequence, respectively. The constraints include control input constraints, control input increment constraints, output constraints, and tire adhesion constraints. The control input constraints, control input increment constraints, and output constraints are used to constrain the upper and lower limits of the control input, control input increment, and output, respectively. The tire adhesion constraint is expressed as follows: ； In the above formula, F long,i and F lat,i These represent the longitudinal and lateral tire forces in the tire coordinate system, respectively, calculated from the tire dynamics model. F z,i For the first i Vertical load on each wheel μ The coefficient of friction of the road surface; The MPC trajectory tracking control problem of the mobile robot is transformed into: solving for the objective function that satisfies the prediction model and the above constraints. J k The control input sequence that achieves the minimum value.

5. The time-domain trajectory tracking control method for a mobile robot according to any one of claims 1-3, characterized in that, The construction of the optimal predictive time-domain control law based on the road surface friction coefficient includes: Under different road surface friction coefficients, different prediction time domain parameters were set to conduct trajectory tracking simulations or experiments to obtain trajectory tracking performance data of mobile robots under different friction coefficients and different prediction time domain combinations. The optimal prediction time-domain parameters corresponding to the friction coefficients of each road surface are determined based on the trajectory tracking performance data, and the correspondence between the road surface friction coefficients and the optimal prediction time-domain parameters is fitted to obtain the optimal prediction time-domain control law based on the road surface friction coefficients.

6. The time-domain trajectory tracking control method for a mobile robot according to claim 5, characterized in that, The process involves determining the optimal prediction time-domain parameters corresponding to each road surface friction coefficient based on trajectory tracking performance data, and fitting the correspondence between the road surface friction coefficient and the optimal prediction time-domain parameters to obtain the optimal prediction time-domain control law based on the road surface friction coefficient. Specifically, this includes: Based on the actual output of the mobile robot obtained from simulation or experiment y k and reference output y ref, k Calculate the trajectory tracking error; A comprehensive evaluation index is constructed based on trajectory tracking error, motion stability, and control input smoothness: ； in, e y This refers to the lateral trajectory error. e θ For heading angle error, e v For the speed error, Δ u To control the input increment, w 1, w 2, w 3, w 4 represents the evaluation weighting coefficient; For the same road surface friction coefficient, select the comprehensive evaluation index. J eva The smallest prediction time domain is taken as the optimal prediction time domain under this friction coefficient; The above process is repeated for multiple road surface friction coefficients to obtain a set of discrete correspondence data between the road surface friction coefficient and the optimal prediction time domain; The PCHIP piecewise cubic Hermite interpolation method is used to fit the above discrete corresponding data, thereby obtaining the optimal predictive time-domain control law based on the road friction coefficient: ； in, N p For prediction in the time domain; It is the interpolation function obtained by fitting the road surface friction coefficient with the optimal prediction time-domain discrete data.

7. The time-domain trajectory tracking control method for a mobile robot according to claim 6, characterized in that, When solving for the optimal prediction time domain based on the road friction coefficient, further... f PCHIP ( μ The output of ) is rounded and limited: ； in, N p, min and N p, max These represent the minimum and maximum values ​​allowed in the prediction time domain, respectively.

8. The time-domain trajectory tracking control method for a mobile robot according to any one of claims 1-3, characterized in that, The method for obtaining the real-time road friction coefficient of the mobile robot during trajectory tracking is as follows: a real-time road friction coefficient estimator is established based on the unscented Kalman filter algorithm. The real-time road friction coefficient estimator uses the road friction coefficient as the state variable to be estimated and estimates the current road friction coefficient in real time based on the state feedback information of the mobile robot and the tire force information. The status feedback information includes at least one of the mobile robot's position, heading angle, longitudinal velocity, lateral velocity, and yaw rate, and the tire force information includes at least one of the tire longitudinal force, tire lateral force, and tire vertical load.

9. The time-domain trajectory tracking control method for a mobile robot according to claim 8, characterized in that, The real-time road friction coefficient estimator uses the road friction coefficients of the four wheels as the state variables to be estimated, and performs a weighted average fusion of the estimated friction coefficients of the four wheels to obtain the road friction coefficients used for predicting time-domain updates in the current control cycle.

10. A time-domain trajectory tracking control system for a mobile robot, characterized in that, include: The prediction model building module is configured to: build a prediction model for time-domain trajectory tracking control of a mobile robot; The trajectory tracking controller building module is configured to: design objective functions and constraints based on the prediction model, and build the MPC trajectory tracking controller for the mobile robot; The optimal prediction time-domain control law construction module is configured to: construct the optimal prediction time-domain control law based on the road surface friction coefficient; The friction coefficient adaptive prediction time domain MPC controller construction module is configured to: combine the MPC trajectory tracking controller with the optimal prediction time domain control law, use the road friction coefficient as the adaptive adjustment variable, and use the prediction time domain parameters as the online adjustment object to construct the friction coefficient adaptive prediction time domain MPC controller for the mobile robot. The trajectory tracking control module of the mobile robot is configured to: acquire the real-time road friction coefficient when the mobile robot is performing trajectory tracking operations and input it into the friction coefficient adaptive prediction time domain MPC controller to dynamically adjust the prediction time domain parameters of the MPC controller, and solve the optimal control quantity of the current control cycle through rolling optimization, and convert the optimal control quantity into the driving torque command and steering angle command of the mobile robot, thereby controlling the mobile robot to track the reference trajectory.

Images