An unmanned mine vehicle trajectory navigation tracking control method and system

Abstract

The application discloses a kind of unmanned mine vehicle trajectory navigation tracking control method and system, method includes: according to the dynamics model of unmanned mine vehicle, trajectory tracking error model and control system state space model are constructed;According to trajectory tracking error model and control system state space model, construct adaptive strategy lateral controller based on linear quadratic regulator LQR and longitudinal controller based on fractional order PID controller;To lateral control law join heading angle compensation, control precision is higher, design adaptive adjustment strategy, according to input saturation function, prevent output saturation, utilize lateral controller to control the steering angle of the front wheel of vehicle;Five parameters of fractional order PID controller are optimized using particle swarm optimization algorithm, the acceleration of vehicle is controlled.

Description

Technical Field

[0001] This application relates to a trajectory navigation and tracking control method and system for unmanned mining vehicles, belonging to the field of unmanned engineering vehicle control technology. Background Technology

[0002] Open-pit mine transportation operations are characterized by planning, organization, and closed systems, and are conducted in harsh environments. In the future vision of intelligent systems for mining enterprises, driverless mining trucks will play a crucial role. However, the inherent nonlinearity and strong coupling of mining vehicles make it difficult to accurately establish mathematical models. The automatic steering system of driverless vehicles occupies a key position in the entire control process, and trajectory tracking control is the most challenging aspect of achieving automatic steering. Trajectory tracking control requires the driverless vehicle to travel stably along a predetermined reference path without deviation. Path planning can be divided into global and local types, with global planning being relatively mature in research. However, trajectory tracking for driverless vehicles still faces challenges in areas such as deviation correction, real-time performance, and system stability.

[0003] In related technologies, in the design of trajectory tracking systems based on state estimation and the study of yaw stability, it is proposed to use the LQR algorithm to design the vehicle lateral controller, and to design the rear wheel yaw stability controller using sliding mode variable structure, and to design the longitudinal speed controller using the PID algorithm. It is simple, reliable and easy to implement.

[0004] However, it has the following drawbacks: the dynamics of unmanned mining vehicles are complex and nonlinear, and it is difficult to ensure that the lateral and longitudinal control of the vehicles achieves the best results. Overshoot and oscillation are prone to occur, and the actual adjustment of parameters depends on experience. The controller is constantly calculating the trajectory error, which occupies computing space. Summary of the Invention

[0005] Objective: In view of at least one of the above technical problems, this application provides a method and system for trajectory navigation and tracking control of unmanned mining vehicles.

[0006] The technical solution adopted in this application is:

[0007] Firstly, this application provides a trajectory navigation and tracking control method for unmanned mining vehicles, including:

[0008] Based on the dynamic model of the unmanned mining vehicle, a trajectory tracking error model and a control system state space model are constructed. Based on the trajectory tracking error model and the control system state space model, an adaptive strategy lateral controller based on a linear quadratic regulator (LQR) and a longitudinal controller based on a fractional PID controller are constructed.

[0009] Obtain the vehicle's actual position, actual speed, and front wheel slip angle. and vehicle heading angle , as well as the desired trajectory reference point position, desired speed, and desired heading angle;

[0010] Calculate the vehicle position error based on the vehicle's current actual position and the desired trajectory reference point position; calculate the speed error based on the vehicle's actual speed and desired speed; calculate the speed error based on the vehicle's heading angle. Calculate the vehicle heading angle error using the expected heading angle. ;

[0011] The actual position, actual speed, and vehicle position error of the vehicle. Vehicle heading angle error Front wheel slip angle and vehicle heading angle An adaptive strategy lateral controller based on a linear quadratic regulator (LQR) is used to control the front wheel steering angle of the vehicle.

[0012] Based on the vehicle's actual position, actual speed, vehicle position error, and speed error, the particle swarm optimization algorithm is used to optimize the five parameters of the fractional-order PID controller to control the vehicle's acceleration.

[0013] In some embodiments, the dynamic model of the unmanned mining vehicle is represented as follows:

[0014] In the global coordinate system Coordinates: Set the center of mass of the unmanned mining vehicle as the origin of the vehicle coordinate system. In the vehicle coordinate system, the length direction of the vehicle body is... Axle, width direction of the car body is Axle, the direction of the car body height is axis;

[0015] Vehicle coordinate system Axial direction: ;

[0016] Vehicle coordinate system Axial direction: ;

[0017] Vehicle coordinate system Axial direction: ;

[0018] in, For the mass of the vehicle body, The longitudinal and lateral velocities of the vehicle in the vehicle coordinate system. , These are the vehicle's longitudinal acceleration and lateral acceleration, respectively. For the sprung mass, The vehicle's tilt angle. The vehicle's roll angle acceleration, For the vehicle's heading angle, Let yaw rate be the vehicle's angular velocity. For vehicle position error, These are the longitudinal moment and the lateral moment acting on the front wheels of the vehicle, respectively. These are the longitudinal moment and the lateral moment acting on the rear wheels of the vehicle, respectively. For the front wheel steering angle, Indicates the vehicle body is around Moment of inertia when the shaft rotates These are the lengths from the front and rear wheels of the vehicle to the center of gravity, respectively. For yaw moment, The vertical distance from the vehicle's center of gravity to the ground. It is the acceleration due to gravity. These are the suspension roll stiffness and roll damping, respectively.

[0019] ;

[0020] ;

[0021] ;

[0022] in, , For the longitudinal lateral stiffness of the front and rear wheels, The longitudinal sideslip ratios of the front and rear wheels. For the lateral stiffness of the front and rear wheels, These are the slip angles of the front and rear wheels, respectively.

[0023] In some embodiments, the trajectory tracking error model is represented as:

[0024] ;

[0025] in, For a moment, For vehicle position error, This refers to the vehicle's heading angle error. For the vehicle's heading angle, The location of the vehicle's center of gravity. , express Vehicle position error and vehicle heading angle error at any given time. express The distance between the vehicle's center of gravity and the reference point of the desired trajectory at any given moment; Used as a reference point for the desired trajectory; for The X and Y coordinates of the desired trajectory reference point at all times. , for The X and Y coordinates of the desired trajectory reference point at all times. They are respectively The X and Y coordinates of the vehicle's center of mass at any given time.

[0026] In some embodiments, the state-space model of the control system is represented as:

[0027] ;

[0028] ;

[0029] ;

[0030] ;

[0031] ;

[0032] in, For state variables, for The first derivative, , , These are the intermediate parameter matrices, For lateral control input. The front wheel slip angle, For the mass of the vehicle body, For the lateral stiffness of the front and rear wheels, These are the lengths from the front and rear wheels of the vehicle to the center of gravity, respectively. Let be the longitudinal velocity of the vehicle in the vehicle coordinate system. Indicates the vehicle body is around Moment of inertia when the shaft rotates This refers to the steering angle of the front wheels.

[0033] In some embodiments, an adaptive policy lateral controller based on a linear quadratic regulator (LQR) includes:

[0034] Lateral control input of the lateral controller Represented as:

[0035] ;

[0036] ;

[0037] ;

[0038] ;

[0039] in, For lateral control input. For state variables; The feedback coefficients of the LQR lateral controller are... These are the status values ​​of the designed auxiliary system. This is the heading angle feedforward value. The front wheel slip angle, This represents the rate of change of the front wheel slip angle. For the mass of the vehicle body, The longitudinal and lateral velocities of the vehicle in the vehicle coordinate system. These are the lengths from the front and rear wheels of the vehicle to the center of gravity, respectively. For the lateral stiffness of the front and rear wheels, For the vehicle's heading angle, The actual curvature of the projection point. It is a positive definite constant. This refers to the vehicle's heading angle error. This refers to the vehicle position error; Input saturation value horizontally. , They are respectively , The horizontal control input at any given time.

[0040] The lateral controller intervenes in control when the following conditions are met:

[0041] ;

[0042] ;

[0043] ;

[0044] ;

[0045] in, , , Let these represent the lateral force function, lateral acceleration function, and error function, respectively. The corresponding functions are respectively , , The weighting coefficients, Indicates the horizontal control threshold; , Under normal working conditions The lateral force and lateral acceleration acting on the vehicle at any given moment. This is a function that evaluates to a value greater than 0; if the input is greater than 0, the output is 1, otherwise the output is 0. As a correction factor, For the mass of the vehicle body, These are the lengths from the front and rear wheels of the vehicle to the center of gravity, respectively. Let be the longitudinal velocity of the vehicle in the vehicle coordinate system. The turning radius of the vehicle. For discrete time step index, , They are respectively , Vehicle position error at any given time , They are respectively , The vehicle heading angle error at any given time;

[0046] In some embodiments, the longitudinal controller employs a fractional-order PID controller, as follows:

[0047] ;

[0048] in, The transfer function of a fractional-order PID controller, Laplace transform for longitudinal control output, The Laplace transform of the longitudinal error signal, , , For PID gain, For fractional integral operators, It is a fractional differential operator.

[0049] In some embodiments, the particle swarm optimization algorithm is used to optimize the five parameters of the fractional-order PID controller. , , , , Optimizations include:

[0050] The transfer function of the fractional-order PID controller is fitted using a fractional-order operator filter;

[0051] The five parameters of the fractional-order PID controller , , , , As the position vector of the particle, the population dimension is Where m is the particle size of the population, the particle swarm optimization algorithm is used for iterative optimization to find the set of vectors with the best fitness values ​​as the final five parameters. , , , , The value of ;

[0052] The formula for calculating the fitness value is as follows:

[0053] ;

[0054] in: For fitness value, for The speed error at any given moment.

[0055] Secondly, this application provides a trajectory navigation and tracking control system for unmanned mining vehicles, including a processor and a storage medium;

[0056] The storage medium is used to store instructions;

[0057] The processor is configured to operate according to the instructions to execute the method according to the first aspect.

[0058] Thirdly, this application provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the method described in the first aspect.

[0059] Fourthly, this application provides a computer device including a memory and a processor, wherein the memory stores a computer program, and the processor executes the computer program to implement the method described in the first aspect.

[0060] Fifthly, this application provides a computer program product, including a computer program that, when executed by a processor, implements the method described in the first aspect.

[0061] Beneficial Effects: The trajectory navigation and tracking control method and system for unmanned mining vehicles provided in this application are based on the LQR algorithm. According to the vehicle's dynamic model, an adaptive strategy lateral controller is designed to calculate the optimal control in the entire control time domain. By adding angle compensation to the lateral control input, the control accuracy is higher. An adaptive adjustment strategy is designed; when the weight ratio of the designed formula exceeds the lateral control threshold, the lateral controller is activated, and output saturation is prevented based on the input saturation function. For the longitudinal controller, a longitudinal trajectory tracking controller based on particle swarm optimization fractional-order PID design is adopted to generate braking and driving torques. The design is more flexible, allowing for the design of the optimal controller to meet the performance requirements of different controlled objects, achieving better control performance. Attached Figure Description

[0062] Figure 1 This is a schematic diagram of the yaw dynamics model of an unmanned mining vehicle according to an embodiment of this application;

[0063] Figure 2This is a schematic diagram of the expected trajectory and actual trajectory of an unmanned mining vehicle in a two-dimensional plane according to an embodiment of this application;

[0064] Figure 3 This is a schematic diagram of the steady-state steering process of an unmanned mining vehicle according to an embodiment of this application;

[0065] Figure 4 This is a schematic diagram of a particle swarm optimization longitudinal controller based on a fractional-order PID according to an embodiment of this application. Detailed Implementation

[0066] The present application will be further described below with reference to the accompanying drawings and embodiments. The following embodiments are only used to more clearly illustrate the technical solutions of the present application, and should not be used to limit the scope of protection of the present application.

[0067] In the description of this application, "several" means one or more, "multiple" means two or more, "greater than," "less than," and "exceeding" are understood to exclude the stated number, while "above," "below," and "within" are understood to include the stated number. The use of "first" and "second" in the description is merely for distinguishing technical features and should not be construed as indicating or implying relative importance, or implicitly indicating the number of indicated technical features, or implicitly indicating the order of the indicated technical features.

[0068] In the description of this application, the terms "one embodiment," "some embodiments," "illustrative embodiment," "example," "specific example," or "some examples," etc., refer to specific features, structures, materials, or characteristics described in connection with that embodiment or example, which are included in at least one embodiment or example of this application. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples.

[0069] The term "and / or" simply describes the relationship between related objects, indicating that three relationships can exist. For example, A and / or B can represent: A alone, A and B simultaneously, or B alone. Additionally, the character " / " generally indicates that the preceding and following related objects have an "or" relationship.

[0070] Example 1: This example provides a trajectory navigation and tracking control method for unmanned mining vehicles, such as... Figure 1 As shown, it includes:

[0071] Based on the dynamic model of the unmanned mining vehicle, a trajectory tracking error model and a control system state space model are constructed. Based on the trajectory tracking error model and the control system state space model, an adaptive strategy lateral controller based on a linear quadratic regulator (LQR) and a longitudinal controller based on a fractional PID controller are constructed.

[0072] Obtain the vehicle's actual position, actual speed, and front wheel slip angle. and vehicle heading angle , as well as the desired trajectory reference point position, desired speed, and desired heading angle;

[0073] Calculate the vehicle position error based on the vehicle's current actual position and the desired trajectory reference point position; calculate the speed error based on the vehicle's actual speed and desired speed; calculate the speed error based on the vehicle's heading angle. Calculate the vehicle heading angle error using the expected heading angle. ;

[0074] The actual position, actual speed, and vehicle position error of the vehicle. Vehicle heading angle error Front wheel slip angle and vehicle heading angle An adaptive strategy lateral controller based on a linear quadratic regulator (LQR) is used to control the front wheel steering angle of the vehicle.

[0075] Based on the vehicle's actual position, actual speed, vehicle position error, and speed error, the particle swarm optimization algorithm is used to optimize the five parameters of the fractional-order PID controller to control the vehicle's acceleration.

[0076] The adaptive lateral controller based on the linear quadratic regulator (LQR) of this application introduces lateral angle compensation, adaptive lateral steering strategy, and anti-input saturation function.

[0077] The longitudinal trajectory tracking controller based on particle swarm optimization fractional-order PID design in this application introduces a fractional-order controller on the basis of PID. For longitudinal velocity, the particle swarm algorithm is used to optimize the parameters and find the optimal set of vectors in space as the optimal solution.

[0078] In this embodiment, a trajectory navigation and tracking control method for unmanned mining vehicles includes:

[0079] (1): Constructing a dynamic model of unmanned mining vehicles

[0080] Compared to vehicles on conventional roads, the working conditions of open-air wide-body dump trucks are relatively harsh. Although the speed is relatively low, the load is large. They often face uphill climbing, narrow roads, loose roads, and excessive turning angles. Conventional geometric and kinematic models are suitable for vehicles with low speed, small load, and small lateral acceleration, but are not suitable for scenarios such as mines where real-time requirements are high, road curvature is large, and accuracy requirements are high. Therefore, dynamic models are required.

[0081] Unmanned mining vehicles in open-pit mines constitute a highly complex system, composed of multiple subsystems such as a power system, steering system, and suspension system. These subsystems exhibit significant coupling relationships, numerous degrees of freedom, and pronounced nonlinear characteristics. Theoretical derivation of this vehicle system becomes complex, making it difficult to establish an accurate mathematical model, while also needing to address errors when processing experimental data. To reduce the computational complexity of the controller, the dynamic model is simplified. To better balance practicality and model complexity in real-world applications, the unmanned mining vehicle is simplified into a system consisting of a car chassis and four wheels. This simplification limits the vehicle's motion freedom to three directions: longitudinal, lateral, and yaw motion.

[0082] Considering model accuracy and complexity, simplify the assumptions:

[0083] (1) Ignore the longitudinal and bow aerodynamic effects.

[0084] (2) Ignore the effect of vehicle tilt.

[0085] (3) Ignore the effects of suspension and coupling relationship, the vehicle body is rigid.

[0086] (4) Ignore longitudinal and lateral tilt during vehicle movement.

[0087] Based on the above assumptions, in the global coordinate system Coordinates: Set the vehicle's center of mass as the origin of the vehicle coordinate system. In the vehicle coordinate system, the length direction of the vehicle body is... Axis (longitudinal), width direction of the vehicle body is Axis (lateral), vehicle body height direction is Axis, such as Figure 1 The figure shows the dynamic model of the unmanned mining vehicle, which is a three-degree-of-freedom motion model.

[0088] Vehicle coordinate system Axial direction:

[0089]

[0090] Vehicle coordinate system Axial direction:

[0091]

[0092] Vehicle coordinate system Axial direction:

[0093]

[0094] in, For the mass of the vehicle body, The longitudinal and lateral velocities of the vehicle in the vehicle coordinate system. , These are the vehicle's longitudinal acceleration and lateral acceleration, respectively. For the sprung mass, The vehicle's tilt angle. The vehicle's roll angle acceleration, For the vehicle's heading angle, Let yaw rate be the vehicle's angular velocity. For vehicle position error, These are the longitudinal moment and the lateral moment acting on the front wheels of the vehicle, respectively. These are the longitudinal moment and the lateral moment acting on the rear wheels of the vehicle, respectively. For the front wheel steering angle, Indicates the vehicle body is around Moment of inertia when the shaft rotates These are the lengths from the front and rear wheels of the vehicle to the center of gravity, respectively. For yaw moment, The vertical distance from the vehicle's center of gravity to the ground. It is the acceleration due to gravity. These are the suspension roll stiffness and roll damping, respectively.

[0095] Because the lateral turning angle fluctuation is small when the vehicle is driving steadily, its deflection angle is... The lateral acceleration is smaller, and the wheel slip angle ranges from [value missing]. Between these points, the torque and the deflection angle are linearly related, from which we can obtain:

[0096]

[0097] in: , This refers to the longitudinal torques acting on the front and rear wheels of the vehicle. , For the longitudinal lateral stiffness of the front and rear wheels, The longitudinal sideslip ratios of the front and rear wheels.

[0098] Similarly, the lateral slip angles of the front and rear wheels have the following linear relationship.

[0099]

[0100] in: , This refers to the lateral moment acting on the front and rear wheels of the vehicle. For the lateral stiffness of the front and rear wheels, These are the slip angles of the front and rear wheels, respectively.

[0101] Regarding the wheel deflection angle, it exists during normal driving, and the longitudinal speed... Greater than or equal to lateral velocity , can be represented as:

[0102]

[0103] in, These are the slip angles of the front and rear wheels, respectively. The longitudinal and lateral velocities of the vehicle in the vehicle coordinate system. Let yaw rate be the vehicle's angular velocity. These are the lengths from the front and rear wheels of the vehicle to the center of gravity, respectively.

[0104] (2) Track tracking control error and state space model of unmanned mining vehicles

[0105] like Figure 2 The image shown is a schematic diagram of the vehicle in a two-dimensional plane. It is a global inertial reference frame.

[0106] By comparing the actual vehicle trajectory with the expected trajectory, the trajectory tracking error can be described as follows:

[0107]

[0108] in, Let be the distance between the vehicle's center of gravity on the desired trajectory and the actual trajectory. The longitudinal and lateral velocities of the vehicle in the vehicle coordinate system. This refers to the vehicle's heading angle error. For the vehicle's heading angle, The heading angle of the trajectory reference point. The actual curvature of the projection point. The coordinates of the arc length of the reference path.

[0109] Along the trajectory direction Differentiating at point , we can obtain:

[0110]

[0111] in, The velocity of the reference point for the desired trajectory. These represent the vehicle's longitudinal and lateral velocities in the vehicle's coordinate system.

[0112] For vehicles traveling under normal road conditions, the offset angle can be ignored, and the trajectory tracking error can be expressed as:

[0113]

[0114] in: Let be the distance between the vehicle's center of gravity on the desired trajectory and the actual trajectory. This refers to the vehicle's heading angle error. The rate of change of the heading angle error. Let yaw rate be the vehicle's angular velocity. Reference path curvature The coordinates of the arc length of the reference path.

[0115] Using binary search to search for path points, for example... Figure 2 Expected trajectory To search for the origin, at the interval Search within a certain distance and calculate the vehicle's center of gravity. Distance from the trajectory search point ,exist The search ends when the time is up, and the found path point becomes the origin of the next trajectory search. Or based on spacing To determine the search range, when the above conditions are met, the origin of the next trajectory search is obtained. And so on.

[0116] According to formula (9), the update formula for the trajectory tracking error function can be obtained:

[0117]

[0118] in, For a moment, For vehicle position error, This refers to the vehicle's heading angle error. For the vehicle's heading angle, The location of the vehicle's center of gravity. , express Vehicle position error and vehicle heading angle error at any given time. express The distance between the vehicle's center of gravity and the reference point of the desired trajectory at any given moment; Used as a reference point for the desired trajectory; for The X and Y coordinates of the desired trajectory reference point at all times. , for The X and Y coordinates of the desired trajectory reference point at all times. They are respectively The X and Y coordinates of the vehicle's center of mass at any given time.

[0119] The state-space model of the control system is represented as:

[0120]

[0121]

[0122]

[0123]

[0124] in, For state variables, for The first derivative, , , These are the intermediate parameter matrices, For lateral control input. The front wheel slip angle, For the mass of the vehicle body, For the lateral stiffness of the front and rear wheels, These are the lengths from the front and rear wheels of the vehicle to the center of gravity, respectively. Let be the longitudinal velocity of the vehicle in the vehicle coordinate system. Indicates the vehicle body is around Moment of inertia when the shaft rotates This refers to the steering angle of the front wheels.

[0125] (3) Adaptive strategy lateral controller based on linear quadratic regulator LQR

[0126] A linear quadratic regulator can control the steering of a vehicle using relatively little energy, allowing the system variable to float around zero. Its formula can be expressed as:

[0127]

[0128] In the formula, Let cost function be For a moment, for The state error vector at time t. for The horizontal control input at any given time. This is the weighting matrix for the lateral controller. R is a positive definite or semi-positive definite matrix, representing the weights of the state variables. R is a positive definite matrix, reflecting the weights of the control variables.

[0129] The desired path is not continuous, but rather the controller tracks the data in a discrete manner. The LQR data is discretized, and the integral can be expressed as:

[0130]

[0131] The discretized equation is:

[0132]

[0133] in:

[0134]

[0135] in, For state variables, for The first derivative, This is the intermediate parameter matrix. For discrete time step index, It is the identity matrix. This is a matrix used for discrete control.

[0136] Q is the identity matrix. The cost function under constraints, constructed using Lagrange multiplication, is as follows:

[0137]

[0138] The formula can be simplified using Hamilton's formula as follows:

[0139]

[0140] Substituting the two equations and simplifying, we get:

[0141]

[0142] To obtain the minimum value of the cost function, we use vector differentiation to find the extreme value of the above equation, resulting in the following equation:

[0143]

[0144] definition, ,get:

[0145]

[0146] in: Let cost function be For discrete time step index, This represents the total number of steps. This is the weighting matrix for the lateral controller. Let Lagrange multiplier vectors be used. For the Ricardi matrix, It is the identity matrix. This is the intermediate parameter matrix.

[0147] The above equation is the Ricardi equation, when After multiple iterations, it can be considered a constant value, and based on the previous vector differentiation formula, we can derive:

[0148]

[0149] Substituting the discretized equations, we obtain the lateral control input:

[0150]

[0151] in: for The horizontal control input at any given time. This is the intermediate parameter matrix. For state variables, For the Ricardi matrix, This is the weighting matrix for the lateral controller. , for , The state variable at any given time.

[0152] It can be simplified to:

[0153]

[0154] in: For the feedback coefficients of the LQR lateral controller;

[0155] Substituting the optimal control into the above equation We can obtain:

[0156]

[0157] Since the lateral heading angle error cannot be zero during vehicle operation, a heading angle feedforward value is added. To compensate for the heading angle error, the input can be rewritten as:

[0158]

[0159] in: , This represents the rate of change of the front wheel slip angle. , , This is the intermediate parameter matrix. This is the heading angle feedforward value. This is the input for lateral control.

[0160] Substituting the matrix expression into the equation yields:

[0161] )

[0162] Finally, the heading angle feedforward value can be obtained. for:

[0163]

[0164] in: The actual curvature of the projection point. For vehicle position error, For the lateral stiffness of the front and rear wheels, These are the lengths from the front and rear wheels of the vehicle to the center of gravity, respectively. The front wheel slip angle, The longitudinal and lateral velocities of the vehicle in the vehicle coordinate system. For the vehicle's heading angle, The mass of the vehicle body.

[0165] When the heading angle reaches the point where the controller needs to intervene, active intervention is performed. If the lateral controller is constantly in operation, it will cause excessive load pressure. Here, an adaptive coordination strategy is used.

[0166] To eliminate the system's transverse input saturation value , The lateral control input affects control performance; the heading angle feedforward value is the input value. Design the following auxiliary system:

[0167]

[0168] In the formula, These are the status values ​​of the designed auxiliary system. It is a positive definite constant (usually) >0, which determines the convergence speed of the auxiliary system. This refers to the vehicle's heading angle error. This represents the vehicle position error.

[0169] The system's lateral input control input is rewritten as follows:

[0170]

[0171] in: For lateral control input. The feedback coefficients of the LQR lateral controller are... For state variables, These are the status values ​​of the designed auxiliary system. This is the heading angle feedforward value.

[0172] like Figure 3 Steady-state steering analysis diagram: During steady-state steering, the lateral force and acceleration of the unmanned mining vehicle are generated by the friction between the tires and the ground. Under normal circumstances, the torque of the front and rear wheels can be described as follows:

[0173]

[0174] The above formula can be simplified to:

[0175]

[0176] In the formula, , This refers to the lateral moment acting on the front and rear wheels of the vehicle. The lateral acceleration experienced by the vehicle. The turning radius of the vehicle. Let be the longitudinal velocity of the vehicle in the vehicle coordinate system. These are the lengths from the front and rear wheels of the vehicle to the center of gravity, respectively. The mass of the vehicle body.

[0177] This application, while fully leveraging the performance of the AFS controller, obtains the centripetal torque and lateral acceleration under normal operating conditions through vehicle sampling. By comparing the error function between the desired trajectory and the actual trajectory, it intervenes with lateral steering control based on an adaptive strategy. The mapping of lateral force and lateral acceleration can be expressed as follows:

[0178]

[0179] In the formula, , , Let these represent the lateral force function, lateral acceleration function, and error function, respectively. The function evaluates the input; if the input is greater than 0, the output is 1; otherwise, the output is 0. A correction factor is added to account for the lateral steady-state characteristics of the tire. The value range is 0-1. Let be the longitudinal velocity of the vehicle in the vehicle coordinate system. These are the lengths from the front and rear wheels of the vehicle to the center of gravity, respectively. For the mass of the vehicle body, The turning radius of the vehicle. This is the index for discrete time steps.

[0180] The tracking error mapping can be represented as:

[0181]

[0182] In the formula, For discrete time step index, , They are respectively , Vehicle position error at any given time , They are respectively , The vehicle heading angle error at any given time; This represents the positional deviation between this moment and the previous moment; similarly, This represents the heading angle error between this moment and the previous moment.

[0183] The lateral controller intervenes when the following conditions are met:

[0184]

[0185] in, , , Let these represent the lateral force function, lateral acceleration function, and error function, respectively. The corresponding functions are respectively , , The weighting coefficients, This is the horizontal control threshold.

[0186] (4): Design of a vertical controller based on fractional-order PID particle swarm optimization

[0187] like Figure 4 The diagram shown is a flowchart of the steps for a fractional-order PID particle swarm optimization longitudinal controller.

[0188] Fractional PID control offers advantages such as simple principle, easy adjustment, and fast response time. The transfer function of the fractional PID controller is:

[0189]

[0190] in, The transfer function of a fractional-order PID controller, Laplace transform for longitudinal control output, The Laplace transform of the longitudinal error signal, , , For PID gain; For fractional integral operators, It is a fractional differential operator.

[0191] Fractional-order operator filters are used to fit the fractional-order system, and frequencies are selected. The fractional order is The filter order is Then the standard form can be expressed as:

[0192]

[0193] In the formula: The zeros, poles, and gain are: , , ,and , imaginary unit .

[0194] Longitudinal control of a vehicle involves comparing its current actual position with the position of a reference point on the desired trajectory to calculate the error in the longitudinal direction. This error is used as input to the longitudinal controller to calculate the acceleration required by the vehicle at the current moment and translate it into corresponding throttle and brake operations, thereby achieving precise control of the vehicle's longitudinal motion. In this control process, the vehicle's error can be divided into two parts: vehicle position error and vehicle speed error. The calculation method for the error is as follows:

[0195]

[0196] In the formula, The actual and desired ordinates of the vehicle. The actual x-coordinate and the expected x-coordinate of the vehicle. For vehicle position error, For the vehicle's heading angle, The actual speed of the vehicle. For speed error, For the desired speed, The velocity of the reference point for the desired trajectory.

[0197] The particle swarm optimization algorithm is used to optimize the parameters of a fractional-order PID controller. In an N-dimensional search space, a swarm of m particles is formed. Each particle can be represented as an N-dimensional vector, where the i-th particle is denoted by xi, and each element xij (j=1,2,...,N) represents its position in the j-th dimension. The position of each particle can be described as a solution to an N-dimensional vector. The position and velocity of particle i at the current moment can be expressed as:

[0198]

[0199] In the formula, For the search space range, For minimum and maximum speeds, No. The particle in the first The position vector of the generation, No. The particle in the first The first generation Dimensional position.

[0200] The five parameters of the fractional-order PID controller , , , , As the position vector of the particle, the population dimension is The process involves particle swarm optimization and iterative iteration to find the set of vectors with the best fitness values. The steps are as follows:

[0201] S1. Define parameters including learning factors c1 and c2, population particle size m, dynamic constant w, minimum fitness value, and maximum number of iterations, which are used to determine the search space range and velocity range.

[0202] S2. Initialize the particle swarm, randomly generate particle swarm velocities and positions, calculate the fitness value of each particle using a fitness function, and determine the global optimal position and the individual optimal position.

[0203]

[0204] in: For fitness value, for The speed error at any given moment.

[0205] S3. Update the position and velocity of each particle, and calculate the fitness value of each particle, i.e.:

[0206]

[0207] In the formula: Learning factor It is a non-negative number. Let w be a set of independent random variables, uniformly distributed in the range [0,1]. Let w be a non-negative inertial weight, called a dynamic variable. For the optimal position of an individual, This is the globally optimal position.

[0208] S4. For each particle, compare its fitness value with the fitness value of the individual best position that the particle has experienced. If the current particle's fitness value is less than the fitness value of the individual best position, then update the particle's position to the individual best position.

[0209] S5. For each particle, compare its fitness value with the fitness value of the global best position. If the fitness value of the current particle is less than the fitness value of the global best position, then update the particle's position to the global best position.

[0210] S6. Update the number of iterations. If the maximum number of iterations is reached or the fitness value of the global optimal position is less than the minimum fitness value, stop the iteration and output the global optimal position, which is the solved parameter value; otherwise, return to S3.

[0211] Example 2: Based on Example 1, this example provides a trajectory navigation and tracking control system for unmanned mining vehicles, including a processor and a storage medium;

[0212] The storage medium is used to store instructions;

[0213] The processor is configured to operate according to the instructions to execute the method according to Embodiment 1.

[0214] Example 3: Based on Example 1, this example provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the method described in Example 1.

[0215] Example 4: Based on Example 1, this example provides a computer device, including a memory and a processor. The memory stores a computer program, and the processor executes the computer program to implement the method described in Example 1.

[0216] Example 5: Based on Example 1, this example provides a computer program product, including a computer program that, when executed by a processor, implements the method described in Example 1.

[0217] Those skilled in the art will understand that embodiments of this application can be provided as methods, systems, or computer program products. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application can take the form of a computer program product embodied on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0218] This application is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this application. It will be understood that each block of the flowchart illustrations and / or block diagrams, as well as combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart... Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0219] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0220] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0221] The above description is only a preferred embodiment of this application. It should be noted that for those skilled in the art, several improvements and modifications can be made without departing from the principle of this application, and these improvements and modifications should also be considered within the scope of protection of this application.

Claims

1. A method for trajectory navigation and tracking control of unmanned mining vehicles, characterized in that, include: Based on the dynamic model of the unmanned mining vehicle, a trajectory tracking error model and a control system state space model are constructed. Based on the trajectory tracking error model and the control system state space model, an adaptive strategy lateral controller based on a linear quadratic regulator (LQR) and a longitudinal controller based on a fractional PID controller are constructed. Obtain the vehicle's actual position, actual speed, front wheel sideslip angle, and vehicle heading angle, as well as the position, expected speed, and expected heading angle of the desired trajectory reference point; Calculate the vehicle position error based on the vehicle's current actual position and the position of the desired trajectory reference point; calculate the speed error based on the vehicle's actual speed and desired speed; calculate the vehicle heading angle error based on the vehicle's heading angle and desired heading angle; The actual position, actual speed, vehicle position error, vehicle heading angle error, front wheel sideslip angle, and vehicle heading angle of the vehicle are input into an adaptive strategy lateral controller based on a linear quadratic regulator (LQR) to control the front wheel steering angle of the vehicle. Based on the vehicle's actual position, actual speed, vehicle position error, and speed error, the particle swarm optimization algorithm is used to optimize the five parameters of the fractional-order PID controller to control the vehicle's acceleration.

2. The method according to claim 1, characterized in that, The dynamic model of the unmanned mining vehicle is expressed as follows: In the global coordinate system Coordinates: Set the center of mass of the unmanned mining vehicle as the origin of the vehicle coordinate system. In the vehicle coordinate system, the length direction of the vehicle body is... Axle, width direction of the car body is Axle, the direction of the car body height is axis; Vehicle coordinate system Axial direction: ; Vehicle coordinate system Axial direction: ; Vehicle coordinate system Axial direction: ; in, For the mass of the vehicle body, The longitudinal and lateral velocities of the vehicle in the vehicle coordinate system. , These are the vehicle's longitudinal acceleration and lateral acceleration, respectively. For the sprung mass, The vehicle's tilt angle. The vehicle's roll angle acceleration, For the vehicle's heading angle, Let yaw rate be the vehicle's angular velocity. For vehicle position error, These are the longitudinal moment and the lateral moment acting on the front wheels of the vehicle, respectively. These are the longitudinal moment and the lateral moment acting on the rear wheels of the vehicle, respectively. For the front wheel steering angle, Indicates the vehicle body is around Moment of inertia when the shaft rotates These are the lengths from the front and rear wheels of the vehicle to the center of gravity, respectively. For yaw moment, The vertical distance from the vehicle's center of gravity to the ground. It is the acceleration due to gravity. These are the suspension roll stiffness and roll damping, respectively.

3. The method according to claim 2, characterized in that, ； ； ； in, , For the longitudinal lateral stiffness of the front and rear wheels, The longitudinal sideslip ratios of the front and rear wheels. For the lateral stiffness of the front and rear wheels, These are the slip angles of the front and rear wheels, respectively.

4. The method according to claim 1, characterized in that, The trajectory tracking error model is expressed as follows: ； in, For a moment, For vehicle position error, This refers to the vehicle's heading angle error. For the vehicle's heading angle, The location of the vehicle's center of gravity. , express Vehicle position error and vehicle heading angle error at any given time. express The distance between the vehicle's center of gravity and the reference point of the desired trajectory at any given moment; Used as a reference point for the desired trajectory; for The X and Y coordinates of the desired trajectory reference point at all times. , for The X and Y coordinates of the desired trajectory reference point at all times. They are respectively The X and Y coordinates of the vehicle's center of mass at any given time.

5. The method according to claim 1, characterized in that, The state-space model of the control system is represented as: ； ； ； ； ； in, For state variables, for The first derivative, , , These are the intermediate parameter matrices, For lateral control input. The front wheel slip angle, For the mass of the vehicle body, For the lateral stiffness of the front and rear wheels, These are the lengths from the front and rear wheels of the vehicle to the center of gravity, respectively. Let be the longitudinal velocity of the vehicle in the vehicle coordinate system. Indicates the vehicle body is around Moment of inertia when the shaft rotates This refers to the steering angle of the front wheels.

6. The method according to claim 1, characterized in that, An adaptive policy lateral controller based on a linear quadratic regulator (LQR) includes: Lateral control input of the lateral controller Represented as: ； ； ； ； in, For lateral control input. For state variables; The feedback coefficients of the LQR lateral controller are... These are the status values ​​of the designed auxiliary system. This is the heading angle feedforward value. The front wheel slip angle, This represents the rate of change of the front wheel slip angle. For the mass of the vehicle body, The longitudinal and lateral velocities of the vehicle in the vehicle coordinate system. These are the lengths from the front and rear wheels of the vehicle to the center of gravity, respectively. For the lateral stiffness of the front and rear wheels, For the vehicle's heading angle, The actual curvature of the projection point. It is a positive definite constant. This refers to the vehicle's heading angle error. This refers to the vehicle position error; Input saturation value horizontally. , They are respectively , The horizontal control input at any given time; The lateral controller intervenes in control when the following conditions are met: ； ； ； ； in, , , Let these represent the lateral force function, lateral acceleration function, and error function, respectively. The corresponding functions are respectively , , The weighting coefficients, Indicates the horizontal control threshold; , Under normal working conditions The lateral force and lateral acceleration acting on the vehicle at any given moment. This is a function that evaluates to a value greater than 0; if the input is greater than 0, the output is 1, otherwise the output is 0. As a correction factor, For the mass of the vehicle body, These are the lengths from the front and rear wheels of the vehicle to the center of gravity, respectively. Let be the longitudinal velocity of the vehicle in the vehicle coordinate system. The turning radius of the vehicle. For discrete time step index, , They are respectively , Vehicle position error at any given time , They are respectively , The vehicle heading angle error at any given time.

7. The method according to claim 1, characterized in that, The longitudinal controller uses a fractional-order PID controller, expressed as: ； in, The transfer function of a fractional-order PID controller, Laplace transform for longitudinal control output, The Laplace transform of the longitudinal error signal, , , For PID gain, For fractional integral operators, It is a fractional differential operator.

8. The method according to claim 7, characterized in that, Five parameters of a fractional-order PID controller were optimized using the particle swarm optimization algorithm. , , , , Optimizations include: The transfer function of the fractional-order PID controller is fitted using a fractional-order operator filter; The five parameters of the fractional-order PID controller , , , , As the position vector of the particle, the population dimension is Where m is the particle size of the population, the particle swarm optimization algorithm is used for iterative optimization to find the set of vectors with the best fitness values ​​as the final five parameters. , , , , The value of ; The formula for calculating the fitness value is as follows: ； in: For fitness value, for The speed error at any given moment.

9. A trajectory navigation and tracking control system for unmanned mining vehicles, characterized in that, Including processor and storage media; The storage medium is used to store instructions; The processor is configured to operate according to the instructions to perform the method according to any one of claims 1 to 8.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it implements the method according to any one of claims 1 to 8.

Images