A method and system for cooperative control of unmanned vehicles on underground mine roads

Abstract

The present invention proposes a method and system for cooperative control of unmanned vehicles on underground mine roads, the method comprising: establishing equations for acceleration of vehicles in all directions according to vehicle driving information; determining the functional relationship between the first measurement parameter represented by the equation and the first input parameter and the first parameter to be estimated according to the equation, and calculating the first Jacobian matrix; obtaining the second measurement variable of the road test through the road test sensor, determining the functional relationship between the second measurement parameter, the second parameter to be estimated and the first input parameter; calculating the second Jacobian matrix; using damped Kalman filtering according to the first Jacobian matrix and the second Jacobian matrix, performing iterative operations to calculate the value of the first parameter to be estimated; and sharing it with other vehicles to realize cooperative control. Based on the method, a control system is also proposed. The present invention calculates the road adhesion coefficient for the strong transient process of tire slippage, and by introducing the damping term, the error suppression of the strong transient variable is realized, thereby improving the vehicle cooperative ability.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned vehicle cooperation in mines, and in particular relates to a method and system for cooperative control of unmanned vehicles on underground mine roads. Background Art

[0002] The slippery road surface caused by the scattering of particulate matter (such as mud or sand) will reduce the vehicle's driving adhesion coefficient, increase the vehicle's longitudinal braking distance and lateral driving stability, and easily cause dangerous conditions such as rear-end collisions and sideslips, endangering the safety of underground vehicle driving.

[0003] Most of the existing solutions are implemented by adding monitoring cameras to identify information such as water accumulation, ice, and spillage, which limits the accurate and refined early warning and management of dangerous conditions such as slippery roads. The management of slippery road conditions mainly relies on cameras to detect the road surface, or by adding laser emission modules, etc., using video detection technology to analyze the reflected laser beams through video algorithms. Therefore, the method of monitoring and warning the road surface through perception solutions such as cameras requires a large number of facilities such as cameras, which requires huge construction and maintenance costs, and is seriously affected by light, weather, and object obstruction. Summary of the invention

[0004] The present invention proposes a method and system for cooperative control of unmanned vehicles on underground mine roads. The adhesion coefficient estimation is deeply integrated with the vehicle-road cooperative system, and the vehicle estimation results are shared with the roadside and the cloud to improve the vehicle cooperative management and control capabilities.

[0005] In order to achieve the above object, the present invention proposes a method for cooperative control of unmanned vehicles on underground mine roads, comprising the following steps:

[0006] Establishing equations for the acceleration of the vehicle in all directions according to the vehicle driving information; the acceleration equations include the acceleration in the forward direction, the acceleration in the vertical forward direction and the vehicle yaw angle acceleration;

[0007] According to the equation of the acceleration of the vehicle in all directions, determine the functional relationship between the first measurement parameter represented by the equation and the first input parameter and the first parameter to be estimated, and calculate the first Jacobian matrix of the first measurement parameter;

[0008] Acquire a second drive test measurement variable through a drive test sensor, determine a functional relationship between a second measurement parameter, a second parameter to be estimated, and the first input parameter; and calculate a second Jacobian matrix of the second measurement parameter;

[0009] According to the first Jacobian matrix and the second Jacobian matrix, an iterative operation is performed using a damped Kalman filter to calculate a first parameter value to be estimated; the calculated first parameter value to be estimated is shared with other vehicles to realize collaborative control.

[0010] Furthermore, the equations for the acceleration of the vehicle in all directions are established based on the vehicle driving information:

[0011] According to the vehicle driving dynamics, we can get:

[0012]

[0013]

[0014]

[0015] Among them, a x 、a y are the longitudinal and lateral accelerations of the vehicle respectively; μ f , μ r are the road adhesion coefficients of the front and rear wheels, respectively; The normalized force provided to the front wheels of the vehicle in the longitudinal direction; The normalized force applied laterally to the front wheels of the vehicle; The normalized force provided to the rear wheels of the vehicle in the longitudinal direction; is the normalized force provided by the rear wheel of the vehicle in the lateral direction; x and y represent the longitudinal and lateral directions of the vehicle respectively, f and r represent the front wheel and rear wheel respectively; m is the vehicle mass; σ is the front wheel turning angle, is the vehicle yaw angular acceleration, l f , l r Respectively represent the distance from the center of mass of the vehicle to the front axle and the rear axle, I z is the yaw moment of inertia of the vehicle.

[0016] Furthermore, according to the equation of the acceleration of the vehicle in all directions, a functional relationship between the first measurement parameter represented by the equation and the first input parameter and the first parameter to be estimated is determined;

[0017] A x 、a y , As the first measurement parameter, it is denoted as y(t);

[0018] The vehicle front wheel steering angle σ is taken as the first input parameter and recorded as u(t); wherein u(t) = [δ];

[0019] μ f , μ r , m is the first parameter to be estimated and is recorded as x p (t); where x p (t)=[u f ,u r ,m] T ;

[0020] Therefore, the first measurement parameter, the first input parameter, and the first parameter to be estimated are abstracted as a function h(); the y(t)=h(x p (t),u(t),w(t)); where w(t) is the first measurement error;

[0021] Calculate h(x p The partial derivative of the first parameter to be estimated is obtained by taking the first Jacobian matrix H(t).

[0022] Furthermore, the process of obtaining the second drive test measurement variable through the drive test sensor and determining the functional relationship between the second measurement parameter, the second parameter to be estimated and the first input parameter is as follows:

[0023] Obtain the second measurement parameter y of the drive test through the drive test sensor _r (t), including a x_r 、a y_r are the acceleration of the vehicle along the longitudinal road and the acceleration of the vehicle along the lateral road respectively; so y _r (t) = h(x p_r (t),u(t),w D (t)); x p_r (t) is the second estimated parameter;

[0024] Calculate h(x p_r (t),u(t),w D (t)) The partial derivative of the second estimated parameter is used to obtain the second Jacobian matrix H corresponding to the second measurement. D (t); where w D (t) Second measurement error.

[0025] Furthermore, the process of performing iterative operation according to the first Jacobian matrix and the second Jacobian matrix by using damped Kalman filtering to calculate the first parameter value to be estimated is:

[0026] The initial value of the iterative calculation is given; a damping term is introduced in the iterative calculation to suppress the error of strong transient variables; the Kalman gain in the Kalman filter is calculated and the first parameter to be estimated in this time step is corrected; after the correction is completed, the time of the time step is updated to the previous time step, and the first parameter to be estimated in the current time step is predicted based on the correction value.

[0027] Furthermore, the given initial value of the iterative calculation includes: randomly giving a first estimated parameter vector x p Initial estimate of (t) And specify a random initial covariance matrix and the initial damped covariance matrix As the initial value for iterative calculation.

[0028] Furthermore, the process of introducing a damping term in the iterative calculation to suppress the error of a strong transient variable is as follows:

[0029] Calculate the damping gain at time t in, is the estimated value of the damping covariance matrix at time t; R D is the Gaussian white noise covariance matrix of the second measurement parameter;

[0030] According to the damping gain, the first measurement parameter y(t) and the second measurement parameter y _r (t), calculate the first measurement parameter y(t) and the second measurement parameter y _r (t) The third measurement parameter y between _D (t); of which:

[0031] Through the effect of damping gain, , thereby achieving error suppression of strong transient variables of the second measurement parameter.

[0032] Furthermore, the process of calculating the Kalman gain in the Kalman filter and correcting the first parameter to be estimated at the current time step includes:

[0033] The formula for calculating the Kalman gain K(t) in the Kalman filter is: in, is the estimated value of the covariance matrix at time t predicted; R is the Gaussian white noise covariance matrix of the first measurement parameter;

[0034] Using the correction equation Calculate the first measurement parameter x at time t p (t); where is the estimate of the first measured parameter at time t; represents y(t)=h(x p (t),u(t),w(t)) is the calculation result when the first input parameter u(t) and the measurement error are 0 at time t;

[0035] Using the state error covariance correction equation

[0036] Update the covariance matrix correction value P(t) at time t and the damping covariance matrix correction value P at time t _D (t).

[0037] Furthermore, the process of updating the time of the time step to the previous time step and predicting the first parameter to be estimated at the current time step according to the correction value includes:

[0038] After the deflection correction at time t, the time step is updated, the calculated time t is changed to time t-1, and the next time is updated to time t;

[0039] According to x p (t-1), u(t), through x p (t) = f(x p (t-1),u(t),0), calculate the estimate of the first measurement parameter at time t

[0040] According to the covariance matrix correction value P(t-1) at time t-1 and the damping covariance matrix correction value at time t And the covariance Q of the excitation noise is used to calculate the estimated covariance matrix at time t and the damped covariance matrix estimate Right now:

[0041] Then perform iterative calculation at time t; finally, we get μ f , μ r , the value of m.

[0042] The present invention also proposes an underground mine road unmanned vehicle cooperative control system, comprising an establishment module, a first determination module, a second determination module and a calculation module;

[0043] The establishment module is used to establish the equations of the acceleration of the vehicle in various directions according to the vehicle driving information; the acceleration equations include the acceleration in the forward direction, the acceleration in the vertical forward direction and the vehicle yaw angle acceleration;

[0044] The first determination module is used to determine the functional relationship between the first measurement parameter represented by the equation and the first input parameter and the first parameter to be estimated according to the equation of the acceleration of the vehicle in all directions, and calculate the first Jacobian matrix of the first measurement parameter;

[0045] The second determination module is used to obtain a second drive test measurement variable through a drive test sensor, determine a functional relationship between a second measurement parameter, a second parameter to be estimated and the first input parameter; and calculate a second Jacobian matrix of the second measurement parameter;

[0046] The calculation module is used to perform iterative operations based on the first Jacobian matrix and the second Jacobian matrix using damped Kalman filtering to calculate the first parameter value to be estimated; and share the calculated first parameter value to be estimated with other vehicles to achieve collaborative control.

[0047] The effects provided in the content of the invention are only the effects of the embodiments, not all the effects of the invention. One of the above technical solutions has the following advantages or beneficial effects:

[0048] The present invention proposes a method and system for cooperative control of unmanned vehicles on underground mine roads. The method includes: establishing equations for acceleration of the vehicle in all directions according to vehicle driving information; the acceleration equations include acceleration in the forward direction, acceleration in the vertical forward direction, and vehicle yaw acceleration; according to the equations for acceleration of the vehicle in all directions, determining the functional relationship between the first measurement parameter represented by the equation and the first input parameter and the first parameter to be estimated, and calculating the first Jacobian matrix of the first measurement parameter; obtaining the second measurement variable of the road test through the road test sensor, determining the functional relationship between the second measurement parameter, the second parameter to be estimated, and the first input parameter; calculating the second Jacobian matrix of the second measurement parameter; according to the first Jacobian matrix and the second Jacobian matrix, using damped Kalman filtering, performing iterative operations, calculating the value of the first parameter to be estimated; sharing the calculated value of the first parameter to be estimated with other vehicles to realize cooperative control. Based on a method for cooperative control of unmanned vehicles on underground mine roads, a cooperative control system for unmanned vehicles on underground mine roads is also proposed. In the algorithm for estimating the road adhesion coefficient, the present invention introduces the measurement of vehicle acceleration and other information by the intelligent roadside, avoiding the errors caused by the traditional method of single measurement through body sensors; for the strong transient process of tire slip, the error suppression of strong transient variables is achieved by introducing the damping term, and a damped Kalman filtering method is established, which improves the adaptability of the filtering algorithm to this working condition; the adhesion coefficient estimation is deeply integrated with the vehicle-road cooperative system, and the vehicle estimation results are shared with the roadside and the cloud, thereby improving the vehicle cooperative management and control capabilities. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] like Figure 1 This is a flow chart of a method for cooperative control of unmanned vehicles on underground mine roads according to Embodiment 1 of the present invention;

[0050] like Figure 2 This is a schematic diagram of the damped Kalman iterative calculation process in Example 1 of the present invention;

[0051] like Figure 3 This is a schematic diagram of an unmanned vehicle collaborative control system for underground mine roads according to embodiment 2 of the present invention. DETAILED DESCRIPTION

[0052] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0053] Example 1

[0054] Embodiment 1 of the present invention proposes a method for cooperative control of unmanned vehicles on underground mine roads, online estimation of road adhesion coefficient based on the damped Kalman filter (DKF) method and road slippery warning based on vehicle-road collaboration. Among them, the damped Kalman filter (DKF) is an algorithm improved and updated with reference to the extended Kalman filter (EKF), which mainly solves the error caused by the strong transient change of the variable when estimating the vehicle speed with a rotation speed, and mainly suppresses the strong transient variable by adding a damping term in the gain matrix.

[0055] like Figure 1 This is a flow chart of a method for cooperative control of unmanned vehicles on underground mine roads according to Embodiment 1 of the present invention;

[0056] In step S100, an equation of the acceleration of the vehicle in each direction is established according to the vehicle driving information; the acceleration equation includes the acceleration in the forward direction, the acceleration in the vertical forward direction and the vehicle yaw angle acceleration;

[0057] According to the vehicle driving dynamics, we can get:

[0058]

[0059]

[0060]

[0061] Among them, a x 、a y are the longitudinal and lateral accelerations of the vehicle respectively; μ f , μ r are the road adhesion coefficients of the front and rear wheels, respectively; The normalized force provided to the front wheels of the vehicle in the longitudinal direction; The normalized force applied laterally to the front wheels of the vehicle; The normalized force provided to the rear wheels of the vehicle in the longitudinal direction; is the normalized force provided by the rear wheel of the vehicle in the lateral direction; x and y represent the longitudinal and lateral directions of the vehicle respectively, f and r represent the front wheel and rear wheel respectively; m is the vehicle mass; σ is the front wheel turning angle, is the vehicle yaw angular acceleration, l f , l r Respectively represent the distance from the center of mass of the vehicle to the front axle and the rear axle, I z is the yaw moment of inertia of the vehicle.

[0062] In step S110, according to the equation of the acceleration of the vehicle in all directions, the functional relationship between the first measurement parameter represented by the equation and the first input parameter and the first parameter to be estimated is determined, and the first Jacobian matrix of the first measurement parameter is calculated;

[0063] In the vehicle driving dynamics equation, because a x 、a y , Provided by the vehicle-mounted inertial navigation element, so a x 、a y , As the first measurement parameter, it is denoted as y(t);

[0064] The vehicle front wheel steering angle σ is taken as the first input parameter and recorded as u(t); wherein u(t) = [δ];

[0065] In order to eliminate the influence of possible vehicle load changes on the estimation results, the vehicle mass m is also taken as the parameter variable to be estimated, so μ f , μ r , m is the first parameter to be estimated and is recorded as x p (t); where x p (t)=[u f ,u r ,m] T ;

[0066] Therefore, the first measured parameter, the first input parameter, and the first parameter to be estimated are abstracted as a function h(); y(t) = h(x p (t),u(t),w(t)); where w(t) is the first measurement error; that is, h(x p (t),u(t),w(t)) represents the functional relationship between the first input parameter, the first parameter to be estimated and the first measurement error. Calculate h(x p The partial derivative of the first parameter to be estimated is obtained by taking the first Jacobian matrix H(t).

[0067] In step S120, a second drive test measurement variable is acquired through a drive test sensor, and a functional relationship among a second measurement parameter, a second parameter to be estimated, and the first input parameter is determined; and a second Jacobian matrix of the second measurement parameter is calculated;

[0068] Obtain vehicle acceleration through road test sensors, including a x_r 、a y_r are the longitudinal acceleration of the vehicle and the lateral acceleration of the vehicle respectively; recorded as: the second measurement parameter y _r (t), so y _r (t) = h(x p_r (t),u(t),w D (t)); x p_r (t) is the second estimated parameter;

[0069] According to the second measurement parameter y _r (t) and the second estimated parameter xp_r (t), the first input parameter u(t) and the second measurement error w D (t), calculate h(x p_r (t),u(t),w D (t)) The partial derivative of the second estimated parameter is used to obtain the second Jacobian matrix H corresponding to the second measurement. D (t); where w D (t) Second measurement error.

[0070] In step S130, an iterative operation is performed based on the first Jacobian matrix and the second Jacobian matrix using a damped Kalman filter to calculate a first parameter value to be estimated; the calculated first parameter value to be estimated is shared with other vehicles to achieve cooperative control. Figure 2 The figure is a schematic diagram of the damped Kalman iterative calculation process in Example 1 of the present invention; the specific process is:

[0071] The initial value of the iterative calculation is given; a damping term is introduced in the iterative calculation to suppress the error of strong transient variables; the Kalman gain in the Kalman filter is calculated and the first parameter to be estimated in this time step is corrected; after the correction is completed, the time of the time step is updated to the previous time step, and the first parameter to be estimated in the current time step is predicted based on the correction value.

[0072] Given the initial value of the iterative calculation, that is Figure 2 The input module in: Randomly given the first estimated parameter vector x p Initial estimate of (t) And specify a random initial covariance matrix and the initial damped covariance matrix As the initial value of the iterative calculation, the iterative calculation then starts.

[0073] The damping term is introduced in the iterative calculation to suppress the error of strong transient variables, that is, Figure 2 The damping module in the implementation process is:

[0074] Calculate the damping gain at time t in, is the estimated value of the damping covariance matrix at time t; R D is the Gaussian white noise covariance matrix of the second measurement parameter;

[0075] According to the damping gain, the first measurement parameter y(t) and the second measurement parameter y _r (t), calculate the first measurement parameter y(t) and the second measurement parameter y _r (t) The third measurement parameter y _D (t); of which:

[0076] Through the effect of damping gain, , thereby achieving error suppression of strong transient variables of the second measurement parameter.

[0077] Calculate the Kalman gain in the Kalman filter and correct the first parameter to be estimated in this time step, that is, Figure 2 The correction module in , the execution process is:

[0078] The formula for calculating the Kalman gain K(t) in the Kalman filter is: in, is the estimated value of the covariance matrix at time t predicted; R is the Gaussian white noise covariance matrix of the first measurement parameter;

[0079] Using the correction equation Calculate the first measurement parameter x at time t p (t); where is the estimate of the first measured parameter at time t; represents y(t)=h(x p (t),u(t),w(t)) is the calculation result when the first input parameter u(t) and the measurement error are 0 at time t;

[0080] Using the state error covariance correction equation

[0081] Update the covariance matrix correction value P(t) at time t and the damping covariance matrix correction value P at time t _D (t).

[0082] Update the time of the time step to the previous time step, and predict the first parameter to be estimated in the current time step according to the correction value, that is, Figure 2 The prediction module in .

[0083] After the correction at time t is completed, the time step is updated, the calculated time t is changed to time t-1, and the next time is updated to time t;

[0084] According to x p (t-1), u(t), through x p (t) = f(x p (t-1),u(t),0), calculate the estimate of the first measurement parameter at time t

[0085] According to the covariance matrix correction value P(t-1) at time t-1 and the damping covariance matrix correction value at time t And the covariance Q of the excitation noise is used to calculate the estimated covariance matrix at time t and the damped covariance matrix estimate Right now: The covariance Q can be given or adjusted according to actual conditions.

[0086] Get the covariance matrix estimate and the damped covariance matrix estimate Afterwards, it is fed back to the damping module and the correction module respectively, and the updated iterative calculation is performed again at time t.

[0087] Through the above calculation process, we can get the estimated value using the first measurement parameter, the second measurement variable, and the covariance matrix and the damped covariance matrix estimate For the first parameter to be estimated x p (t), that is, μ f , μ r , the value of m.

[0088] In getting μ f , μ r , m, the adhesion coefficient μ f , μ r The value represents the adhesion condition of the road surface on which the vehicle is traveling at this time. When the adhesion coefficient is about 0.85, it indicates that the vehicle is on a dry and good road surface. When it is 0.2-0.4, it may be on a relatively slippery road surface. When it is less than 0.2, it indicates high-risk conditions such as ice and sand. The data protected by the present invention are not limited to the specific data listed in Example 1, and those skilled in the art can set or determine according to actual conditions.

[0089] According to the specific values, it can be sent to the OBU terminal of other vehicles or the roadside RSU terminal through the on-board OBU, and broadcast by the RSU to the vehicle end (the communications involved include but are not limited to 4G-LTE, 5G, DSRC and other communication means). After other vehicles receive the relevant information, they can provide early warning information to the driver in the form of sound, text, etc., so that the driver can slow down in advance, pay attention to vehicle operation, etc. to ensure driving safety. At the same time, it can be transmitted by the RSU to the corresponding back-end management and control platform to provide reference for underground traffic control, etc., to realize vehicle collaborative control based on vehicle-road collaboration.

[0090] A method for cooperative control of unmanned vehicles on underground mine roads proposed in Example 1 of the present invention introduces intelligent roadside measurement of vehicle acceleration and other information in the algorithm for estimating the road adhesion coefficient, thereby avoiding the error caused by the traditional method of measuring only through vehicle body sensors;

[0091] Embodiment 1 of the present invention aims at the strong transient process of tire slippage, and realizes error suppression of strong transient variables by introducing a damping term, establishes a damped Kalman filtering method, and improves the adaptability of the filtering algorithm to the working condition;

[0092] In embodiment 1 of the present invention, the adhesion coefficient estimation is deeply integrated with the vehicle-road cooperative system, and the vehicle estimation result is shared with the roadside and the cloud, thereby improving the vehicle cooperative management and control capabilities.

[0093] Example 2

[0094] Based on the underground mine road unmanned vehicle cooperative control method proposed in Example 1 of the present invention, Example 2 of the present invention further proposes an underground mine road unmanned vehicle cooperative control system, such as Figure 3 This is a schematic diagram of an underground mine road unmanned vehicle cooperative control system proposed in Example 2 of the present invention, the system comprising: an establishment module, a first determination module, a second determination module and a calculation module;

[0095] The building module is used to build the equations of the acceleration of the vehicle in all directions according to the vehicle driving information; the acceleration equations include the acceleration in the forward direction, the acceleration in the vertical forward direction and the vehicle yaw angle acceleration;

[0096] The first determination module is used to determine the functional relationship between the first measurement parameter represented by the equation and the first input parameter and the first parameter to be estimated according to the equation of the acceleration of the vehicle in all directions, and calculate the first Jacobian matrix of the first measurement parameter;

[0097] The second determination module is used to obtain a second measurement variable of the drive test through the drive test sensor, determine the functional relationship between the second measurement parameter, the second parameter to be estimated and the first input parameter; and calculate the second Jacobian matrix of the second measurement parameter;

[0098] The calculation module is used to perform iterative operations based on the first Jacobian matrix and the second Jacobian matrix using damped Kalman filtering to calculate the first parameter value to be estimated; and share the calculated first parameter value to be estimated with other vehicles to achieve collaborative control.

[0099] An underground mine road unmanned vehicle cooperative control system proposed in Example 2 of the present invention modularizes the process implemented by the method of Example 1, and the specific implementation process refers to the process implemented by the method of Example 1.

[0100] In an underground mine road unmanned vehicle cooperative control system proposed in Example 2 of the present invention, an intelligent roadside device is introduced to measure information such as vehicle acceleration in the algorithm for estimating the road adhesion coefficient, thereby avoiding the error caused by the traditional method of measuring only through the vehicle body sensor.

[0101] Embodiment 2 of the present invention aims at the strong transient process of tire slippage, and realizes error suppression of strong transient variables by introducing a damping term, establishes a damped Kalman filtering method, and improves the adaptability of the filtering algorithm to this working condition;

[0102] In embodiment 2 of the present invention, the adhesion coefficient estimation is deeply integrated with the vehicle-road cooperative system, and the vehicle estimation results are shared with the roadside and the cloud, thereby improving the vehicle cooperative management and control capabilities.

[0103] It should be noted that, in this article, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply that there is any such actual relationship or order between these entities or operations. Moreover, the term "include", "comprise" or any other variant thereof is intended to cover non-exclusive inclusion, so that the process, method, article or equipment that includes a series of elements are inherent to the elements. In the absence of more restrictions, the elements limited by the sentence "comprise one..." do not exclude the presence of other identical elements in the process, method, article or equipment that includes the elements. In addition, the above-mentioned technical solution provided in the embodiment of the present application is consistent with the corresponding technical solution in the prior art in principle, and the part is not described in detail, so as not to repeat too much.

[0104] Although the above describes the specific implementation of the present invention in conjunction with the accompanying drawings, it is not intended to limit the scope of protection of the present invention. For those skilled in the art, other different forms of modifications or deformations can be made on the basis of the above description. It is not necessary and impossible to list all the implementation methods here. On the basis of the technical solution of the present invention, various modifications or deformations that can be made by those skilled in the art without creative work are still within the scope of protection of the present invention.

Claims

1. A method for cooperative control of unmanned vehicles on underground mine roads, characterized in that: The following steps are involved: Establishing equations for the acceleration of the vehicle in all directions according to the vehicle driving information; the acceleration equations include the acceleration in the forward direction, the acceleration in the direction perpendicular to the forward direction and the vehicle yaw angle acceleration; According to the equation of the acceleration of the vehicle in all directions, determine the functional relationship between the first measurement parameter represented by the equation and the first input parameter and the first parameter to be estimated, and calculate the first Jacobian matrix of the first measurement parameter; Acquire a second drive test measurement variable through a drive test sensor, and determine a functional relationship between a second measurement parameter, a second parameter to be estimated, and the first input parameter; Calculate the second Jacobian matrix of the second measurement parameter; obtain the second measurement variable of the drive test through the drive test sensor, and determine the functional relationship between the second measurement parameter, the second parameter to be estimated and the first input parameter. The process is: obtain the second measurement parameter y of the drive test through the drive test sensor _r (t), including a x_r 、a y_r are the acceleration of the vehicle along the longitudinal road and the acceleration of the vehicle along the lateral road respectively; so y _r (t) = h(x p_r (t),u(t),w D (t)); x p_r (t) is the second estimated parameter; calculate h(x p_r (t),u(t),w D (t)) The partial derivative of the second estimated parameter is used to obtain the second Jacobian matrix H corresponding to the second measurement. D (t); where w D (t) is the second measurement error; u(t) is the first input parameter; u(t) = [δ]; σ is the front wheel steering angle of the vehicle; According to the first Jacobian matrix and the second Jacobian matrix, using damped Kalman filtering, performing an iterative operation to calculate a first parameter value to be estimated; The calculated first parameter value to be estimated is shared with other vehicles to realize cooperative control; the process of calculating the first parameter value to be estimated by iterative operation based on the first Jacobian matrix and the second Jacobian matrix using damped Kalman filter is as follows: giving an initial value of the iterative calculation; introducing a damping term in the iterative calculation to suppress the error of strong transient variables; calculating the Kalman gain in the Kalman filter and correcting the first parameter to be estimated at this time step; after the correction is completed, updating the time step to the previous time step, and predicting the first parameter to be estimated at the current time step according to the correction value.

2. The method for cooperative control of unmanned vehicles on underground mine roads according to claim 1, characterized in that: The equations for the vehicle's acceleration in all directions are established based on the vehicle's driving information: According to the vehicle driving dynamics, we can get: Among them, a x 、a y are the longitudinal and lateral accelerations of the vehicle respectively; μ f is the road adhesion coefficient of the front wheel, μ r is the road adhesion coefficient of the rear wheel; The normalized force provided to the front wheels of the vehicle in the longitudinal direction; The normalized force applied laterally to the front wheels of the vehicle; The normalized force provided to the rear wheels of the vehicle in the longitudinal direction; is the normalized force provided by the rear wheel of the vehicle in the lateral direction; x and y represent the longitudinal and lateral directions of the vehicle respectively, f and r represent the front wheel and rear wheel respectively; m is the vehicle mass; σ is the front wheel turning angle, is the vehicle yaw angular acceleration, l f , l r Respectively represent the distance from the center of mass of the vehicle to the front axle and the rear axle, I z is the yaw moment of inertia of the vehicle.

3. The method for cooperative control of unmanned vehicles on underground mine roads according to claim 2, characterized in that: The function relationship between the first measurement parameter represented by the equation and the first input parameter and the first parameter to be estimated is determined according to the equation of the acceleration of the vehicle in all directions; A x 、a y , As the first measurement parameter, it is denoted as y(t); The vehicle front wheel steering angle σ is taken as the first input parameter and recorded as u(t); wherein u(t) = [δ]; μ f , μ r , m is the first parameter to be estimated and is recorded as x p (t); where x p (t)=[u f ,u r ,m] T ; Therefore, the first measurement parameter, the first input parameter, and the first parameter to be estimated are abstracted as a function h(); the y(t)=h(x p (t),u(t),w(t)); where w(t) is the first measurement error; Calculate h(x p The partial derivative of the first parameter to be estimated is obtained by taking the first Jacobian matrix H(t).

4. The method for cooperative control of unmanned vehicles on underground mine roads according to claim 1, characterized in that: The initial value of the given iterative calculation includes: randomly giving a first estimated parameter vector x p Initial estimate of (t) And specify a random initial covariance matrix and the initial damped covariance matrix As the initial value for iterative calculation.

5. The method for cooperative control of unmanned vehicles on underground mine roads according to claim 4, characterized in that: The process of introducing the damping term in the iterative calculation to suppress the error of the strong transient variable is as follows: Calculate the damping gain at time t in, is the estimated value of the damping covariance matrix at time t; R D is the Gaussian white noise covariance matrix of the second measurement parameter; According to the damping gain, the first measurement parameter y(t) and the second measurement parameter y _r (t), calculate the first measurement parameter y(t) and the second measurement parameter y _r (t) The third measurement parameter y _D (t); of which: Through the effect of damping gain, , thereby achieving error suppression of strong transient variables of the second measurement parameter.

6. The method for cooperative control of unmanned vehicles on underground mine roads according to claim 5, characterized in that: The process of calculating the Kalman gain in the Kalman filter and correcting the first parameter to be estimated at the current time step includes: The formula for calculating the Kalman gain k(t) in the Kalman filter is: in, is the estimated value of the covariance matrix at time t predicted; R is the Gaussian white noise covariance matrix of the first measurement parameter; Using the correction equation Calculate the first measurement parameter x at time t p (t); where is the estimate of the first measured parameter at time t; represents y(t)=h(x p (t),u(t),w(t)) is the calculation result when the first input parameter u(t) and the measurement error are 0 at time t; Using the state error covariance correction equation Update the covariance matrix correction value P(t) at time t and the damping covariance matrix correction value P at time t _D (t).

7. The method for cooperative control of unmanned vehicles on underground mine roads according to claim 6, characterized in that: The process of updating the time of the time step to the previous time step and predicting the first parameter to be estimated in the current time step according to the correction value includes: After the correction at time t is completed, the time step is updated, the calculated time t is changed to time t-1, and the next time is updated to time t; According to x p (t-1), u(t), through x p (t) = f(x p (t-1),u(t),0), calculate the estimate of the first measurement parameter at time t According to the covariance matrix correction value P(t-1) at time t-1 and the damping covariance matrix correction value at time t And the covariance Q of the excitation noise is used to calculate the estimated covariance matrix at time t and the damped covariance matrix estimate Right now: Then perform iterative calculation at time t; finally, we get μ f , μ r , the value of m.

8. An underground mine road unmanned vehicle cooperative control system, used to execute an underground mine road unmanned vehicle cooperative control method according to any one of claims 1 to 7, characterized in that: include: An establishment module, a first determination module, a second determination module and a calculation module; The establishment module is used to establish equations for the acceleration of the vehicle in all directions according to the vehicle driving information; The acceleration equation includes the acceleration in the forward direction, the acceleration in the direction perpendicular to the forward direction and the vehicle yaw angular acceleration; The first determination module is used to determine the functional relationship between the first measurement parameter represented by the equation and the first input parameter and the first parameter to be estimated according to the equation of the acceleration of the vehicle in all directions, and calculate the first Jacobian matrix of the first measurement parameter; The second determination module is used to obtain a second drive test measurement variable through a drive test sensor, and determine a functional relationship between a second measurement parameter, a second parameter to be estimated, and the first input parameter; calculating a second Jacobian matrix of a second measurement parameter; The calculation module is used to perform iterative operation according to the first Jacobian matrix and the second Jacobian matrix by using damped Kalman filtering to calculate the first parameter value to be estimated; The calculated first parameter value to be estimated is shared with other vehicles to achieve collaborative control.

Images