Path tracking control system and method and wheel loader

Abstract

The invention discloses a path tracking control system and method and a wheel loader, and belongs to the technical field of path tracking, the path tracking control system comprises an industrial personal computer, the industrial personal computer is electrically connected with a laser radar and a whole vehicle controller, and the laser radar is electrically connected with the whole vehicle controller. The vehicle control unit is electrically connected with the hinge angle sensor, the steering oil pump oil way pressure sensor, the servo steering oil pump oil way pressure sensor, the rod cavity pressure sensor, the rodless cavity pressure sensor, the electromagnetic reversing valve, the unmanned operation electromagnetic valve, the electromagnetic proportional reversing valve and the motor controller. And the motor controller is electrically connected with the hub motor. Electro-hydraulic double-loop control is adopted in a wheel loader hardware architecture, a redundancy function is achieved, real-time data of the laser radar are collected through the industrial personal computer, real-time data of the hinge angle sensor and the oil way pressure sensor are collected through the whole vehicle controller, the whole vehicle controller controls four hub motors and servo of the loader, and the loading precision of the loader is improved. Therefore, the real-time path tracking operation of the loader is realized.

Description

Technical Field

[0001] The invention discloses a path tracking control system, a method and a wheel loader, belonging to the technical field of path tracking. Background Art

[0002] Wheel loaders have the advantages of large load capacity, strong adaptability, high reliability, etc., and have become one of the main production and transportation tools in engineering construction, and are widely used in unstructured terrains such as mines, mixing plants, and construction sites. The operating environment of wheel loaders is harsh, the operation is highly repetitive, and the operation intensity is large. Drivers are prone to driving fatigue, resulting in reduced work efficiency and even safety accidents, causing life and property losses. Therefore, how to liberate the loader driver from high-risk and highly repetitive operations has become an urgent problem to be solved.

[0003] At present, due to the unique steering structure and specialized operation scenarios of wheel loaders, the path planning and path tracking algorithms of passenger cars cannot be applied to wheel loaders. Summary of the Invention

[0004] The purpose of the invention is to solve the problem that the existing loader drivers are prone to driving fatigue, resulting in reduced work efficiency, and to propose a path tracking control system, a method and a wheel loader.

[0005] The problems to be solved by the invention are realized by the following technical solutions:

[0006] A path tracking control system includes an industrial control computer, which is electrically connected to a lidar and a vehicle controller respectively. The vehicle controller is electrically connected to a hinge angle sensor, a steering oil pump oil pressure sensor, a servo steering oil pump oil pressure sensor, a rod chamber pressure sensor, a rodless chamber pressure sensor, an electromagnetic directional valve, an unmanned operation solenoid valve, an electro-hydraulic proportional directional valve and a motor controller respectively. The motor controller is electrically connected to a hub motor.

[0007] Further, the motor controller includes a front vehicle body left motor controller, a front vehicle body right motor controller, a rear vehicle body left motor controller and a rear vehicle body right motor controller. The hub motor includes:

[0008] A front vehicle body left hub motor, which is electrically connected to the front vehicle body left motor controller;

[0009] A front vehicle body right hub motor, which is electrically connected to the front vehicle body right motor controller;

[0010] A rear vehicle body left hub motor, which is electrically connected to the rear vehicle body left motor controller;

[0011] A rear vehicle body right hub motor, which is electrically connected to the rear vehicle body right motor controller.

[0012] Further, the electromagnetic directional valve is respectively communicated with a servo steering oil pump, a priority valve and an unmanned operation solenoid valve. The servo steering oil pump is respectively communicated with a servo steering oil pump filter, a servo steering oil pump oil circuit pressure sensor and a relief valve. The unmanned operation solenoid valve is respectively communicated with an electromagnetic proportional directional valve. The electromagnetic proportional directional valve, the priority valve, the relief valve and the servo steering oil pump filter are all communicated with an artificial driving hydraulic steering system.

[0013] Further, the artificial driving hydraulic steering system includes a left steering hydraulic cylinder, a right steering hydraulic cylinder and a hydraulic oil tank which are respectively communicated with the electromagnetic proportional directional valve. Rod chamber pressure sensors are respectively connected to the rod chambers of the left steering hydraulic cylinder and the right steering hydraulic cylinder. Rodless chamber pressure sensors are respectively connected to the rodless chambers of the left steering hydraulic cylinder and the right steering hydraulic cylinder. The left steering hydraulic cylinder, the right steering hydraulic cylinder and the hydraulic oil tank are respectively communicated with a steering gear. The steering gear is communicated with the unmanned operation solenoid valve. The hydraulic oil tank is respectively communicated with the relief valve, a steering oil pump filter, a servo steering oil pump filter and the priority valve. The steering oil pump filter is communicated with a steering oil pump. The steering oil pump is respectively communicated with the priority valve, the relief valve and a steering oil pump oil circuit pressure sensor.

[0014] A path tracking control method, applied to the above path tracking control system, includes:

[0015] The industrial control computer obtains the initial position and the end position of the loader operation, performs attitude normalization setting on the wheel loader to obtain the converted initial position coordinates and end position coordinates, and executes a path planning strategy according to the converted initial position coordinates and end position coordinates to send the optimal safe path coordinate information to the vehicle controller;

[0016] The vehicle controller obtains the current vehicle speed and executes a path tracking control strategy based on Tube-MPC according to the optimal safe path coordinate information and the reference vehicle speed;

[0017] The industrial control computer calculates the relative distance according to the real-time positioning information of the lidar and the end position set on the optimal safe path. When the distance between the real-time positioning information and the end position is less than or equal to a preset end stop distance, the control process ends.

[0018] Further, the obtaining the initial position and the end position of the loader operation and performing attitude normalization setting on the wheel loader includes:

[0019] Set the initial position coordinates and end position coordinates in the geodetic coordinate system;

[0020] Establish a vehicle coordinate system with the initial position coordinates, and convert and normalize the end position coordinates based on the vehicle coordinate system to obtain the converted initial position coordinates and end position coordinates.

[0021] Further, the optimal safe path coordinate information obtained by executing a path planning strategy according to the converted initial position coordinates and end position coordinates is sent to the vehicle controller, including:

[0022] Perform path planning based on the converted initial position coordinates and end position coordinates to obtain path coordinate information;

[0023] Execute an adaptive safe path addition strategy according to the path coordinate information to obtain the optimal safe path coordinate information and send it to the vehicle controller.

[0024] Further, the optimal safe path coordinate information obtained by executing an adaptive safe path addition strategy according to the path coordinate information and sent to the vehicle controller includes:

[0025] Add an adaptive safe path at the initial position, end position, and turning point positions in the path according to the path coordinate information to obtain the first safe path point coordinate information;

[0026] Obtain the first distances from multiple points on the path to the boundary points through the first safe path point coordinate information;

[0027] Compare the first distances from multiple points on the path to the boundary points to obtain the first minimum distance from the points on the path to the boundary points;

[0028] Judge whether the first minimum distance is less than the safety boundary value:

[0029] Yes, execute the next step;

[0030] No, the first safe path coordinate information is the optimal safe path coordinate information and is sent to the vehicle controller;

[0031] Modify the adaptive turning radius of the first safe path coordinate information to obtain the second safe path point coordinate information;

[0032] Obtain the second distances from multiple points on the path to the boundary points through the second safe path point coordinate information;

[0033] Compare the second distances from multiple points on the path to the boundary points to obtain the second minimum distance from the points on the path to the boundary points;

[0034] Judge whether the second minimum distance is less than the safety boundary value:

[0035] Yes, execute the next step;

[0036] No, the second safety path coordinate information is sent to the vehicle controller as the optimal safety path coordinate information;

[0037] Optimize the straight-line lengths of the first and last segments of the first safety path coordinate information to obtain the third safety path point coordinate information;

[0038] Obtain the third distances from the points on multiple paths to the boundary points through the third safety path point coordinate information;

[0039] Compare the third distances from the points on multiple paths to the boundary points to obtain the third minimum distance from the points on the path to the boundary points;

[0040] Judge whether the third minimum distance is less than the safety boundary value:

[0041] Yes, execute the next step;

[0042] No, the third safety path coordinate information is sent to the vehicle controller as the optimal safety path coordinate information;

[0043] Correct the starting point allowable error of the first safety path coordinate information to obtain the fourth safety path point coordinate information;

[0044] Obtain the fourth distances from the points on multiple paths to the boundary points through the fourth safety path point coordinate information;

[0045] Compare the fourth distances from the points on multiple paths to the boundary points to obtain the fourth minimum distance from the points on the path to the boundary points;

[0046] Judge whether the fourth minimum distance is less than the safety boundary value:

[0047] Yes, give an error prompt and display that the current working space is narrow and a safe path cannot be planned. Please modify the safety boundary value and re-add an adaptive safety path at the initial position, end position, and turning point positions of the path according to the path coordinate information;

[0048] No, the fourth safety path coordinate information is sent to the vehicle controller as the optimal safety path coordinate information.

[0049] Further, before the vehicle controller obtains the current vehicle speed and executes the path tracking control strategy based on Tube-MPC according to the optimal safety path coordinate information and the reference vehicle speed, it further includes:

[0050] Judge whether the unmanned operation solenoid valve signal is powered on:

[0051] Yes, obtain the current vehicle speed and execute the path tracking control strategy based on Tube-MPC according to the optimal safety path coordinate information and the reference vehicle speed;

[0052] No, the control system is operated by the driver.

[0053] A wheel loader includes the above path tracking control system.

[0054] The present invention discloses a path tracking control system, method and wheel loader, which have the following beneficial effects compared with the prior art:

[0055] (1) In the hardware architecture of the wheel loader, an electric and hydraulic dual-loop control is adopted, which has a redundant function. The industrial control computer collects the real-time data of the lidar, and the vehicle controller collects the real-time data of the articulation angle sensor and the oil circuit pressure sensor, and the vehicle controller controls the four hub motors and the servo of the loader to realize the real-time path tracking operation of the loader;

[0056] (2) In the hardware architecture of the wheel loader, an unmanned operation solenoid valve is adopted to realize the switching between unmanned operation and driver manual operation of the wheel loader, improving the practicability of the wheel loader;

[0057] (3) By obtaining the position information collected in real time by the lidar, using Reeds-Shepp path planning, and carrying out an optimal path design with an adaptive safety path addition, an adaptive turning radius modification, an optimization of the straight line length of the first and last segments, and a starting point tolerance correction algorithm for the operating conditions of the wheel loader, the effectiveness and safety of the autonomous operation of the wheel loader are ensured;

[0058] (4) Based on the data collected in real time by the articulation angle sensor, the oil circuit pressure sensor and the lidar, a path tracking algorithm based on Tube model predictive control is designed to calculate the required articulation angular velocity and vehicle speed during the path tracking process;

[0059] (5) Carry out path tracking control on the wheel loader, and carry out autonomous operation by controlling its steering hydraulic cylinder and hub motor, improving the robustness and reliability of the wheel loader and ensuring the efficiency of autonomous operation. Description of the Drawings

[0060] Figure 1 It is a schematic diagram of the hardware architecture of a path tracking control system of the present invention;

[0061] Figure 2 It is an electrical structure diagram of a path tracking control system of the present invention;

[0062] Figure 3 It is a pipeline structure diagram of a path tracking control system of the present invention;

[0063] Figure 4 is a flowchart of a path tracking control method of the present invention;

[0064] Figure 5 is a schematic diagram of adding an adaptive safety path in a path tracking control method of the present invention.

[0065] Figure 6 is a schematic diagram of modifying an adaptive turning radius in a path tracking control method of the present invention.

[0066] Figure 7 is a schematic diagram of optimizing the length of the straight lines at the beginning and end in a path tracking control method of the present invention.

[0067] Figure 8 is a schematic diagram of correcting the allowable error at the starting point in a path tracking control method of the present invention. Detailed implementation manners

[0068] The following further describes the present invention according to the attached Figure 1-8 :

[0069] The technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are some but not all of the embodiments of the present invention. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0070] In the description of the present invention, it should be noted that the orientation or positional relationship indicated by the terms "center", "upper", "lower", "left", "right", "vertical", "horizontal", "inner", "outer", etc. is based on the orientation or positional relationship shown in the drawings, and is only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore should not be construed as a limitation of the present invention.

[0071] In the description of the present invention, it should be noted that unless otherwise clearly specified and limited, the terms "installed", "connected", "connected" should be understood in a broad sense. For example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be directly connected or indirectly connected through an intermediate medium, and it can be the communication inside two elements. For those of ordinary skill in the art, the specific meanings of the above terms in the present invention can be understood according to specific situations.

[0072] Such as Figure 1As shown in the figure, the first embodiment of the present invention provides a path tracking control system on the basis of the prior art, including an industrial control computer, which is electrically connected to a lidar and a vehicle controller respectively. The vehicle controller is electrically connected to a hinge angle sensor, a steering oil pump oil pressure sensor, a servo steering oil pump oil pressure sensor, a rod chamber pressure sensor, a rodless chamber pressure sensor, an electromagnetic reversing valve, an unmanned operation solenoid valve, an electro-hydraulic proportional reversing valve and a motor controller respectively. The motor controller is electrically connected to a hub motor. The motor controller includes a left front vehicle body motor controller, a right front vehicle body motor controller, a left rear vehicle body motor controller and a right rear vehicle body motor controller. The hub motor includes:

[0073] A left front vehicle body hub motor, which is electrically connected to the left front vehicle body motor controller;

[0074] A right front vehicle body hub motor, which is electrically connected to the right front vehicle body motor controller;

[0075] A left rear vehicle body hub motor, which is electrically connected to the left rear vehicle body motor controller;

[0076] A right rear vehicle body hub motor, which is electrically connected to the right rear vehicle body motor controller.

[0077] As Figure 2 shown, the electromagnetic reversing valve is respectively connected to a servo steering oil pump, a priority valve and an unmanned operation solenoid valve. The servo steering oil pump is respectively connected to a servo steering oil pump filter, a servo steering oil pump oil pressure sensor and an overflow valve. The unmanned operation solenoid valve is respectively connected to an electro-hydraulic proportional reversing valve. The electro-hydraulic proportional reversing valve, the priority valve, the overflow valve and the servo steering oil pump filter are all connected to a manual driving hydraulic steering system.

[0078] The manual driving hydraulic steering system includes a left steering hydraulic cylinder, a right steering hydraulic cylinder and a hydraulic oil tank which are respectively connected to the electro-hydraulic proportional reversing valve. The rod chambers of the left steering hydraulic cylinder and the right steering hydraulic cylinder are respectively connected with a rod chamber pressure sensor. The rodless chambers of the left steering hydraulic cylinder and the right steering hydraulic cylinder are respectively connected with a rodless chamber pressure sensor. The left steering hydraulic cylinder, the right steering hydraulic cylinder and the hydraulic oil tank are respectively connected to a steering gear. The steering gear is connected to an unmanned operation solenoid valve. The hydraulic oil tank is respectively connected to an overflow valve, a steering oil pump filter, a servo steering oil pump filter and a priority valve. The steering oil pump filter is connected to a steering oil pump. The steering oil pump is respectively connected to a priority valve, an overflow valve and a steering oil pump oil pressure sensor.

[0079] In this embodiment, the manual driving "electric - hydraulic" steering system includes: a servo steering oil pump oil pressure sensor, a rod chamber pressure sensor, a rodless chamber pressure sensor, a steering gear, a steering wheel, a hydraulic oil tank, a servo steering oil pump filter, a servo steering oil pump, a left steering hydraulic cylinder, and a right steering hydraulic cylinder. Thus, dual - loop control of electricity and hydraulics is adopted in the hardware architecture of the wheel loader to achieve redundant functions.

[0080] As Figure 3 and 4 shown, based on the first embodiment, the second embodiment of the present invention provides a path tracking control method, including:

[0081] Step S10, the industrial control computer obtains the initial position and the end position of the loader operation, performs normalization setting on the attitude of the wheel loader to obtain the converted initial position coordinates and end position coordinates, and executes a path planning strategy according to the converted initial position coordinates and end position coordinates to send the optimal safe path coordinate information to the vehicle controller. The specific steps are as follows:

[0082] Step S11, set the initial position coordinates and end position coordinates in the geodetic coordinate system. The specific content is as follows:

[0083] In the geodetic coordinate system XOY, the coordinates of the initial position A are (x 0 , y 0 , θ 0 ), and the coordinates of the end position B are (x n , y n , θ n ). Among them, θ 0 , θ n represent the included angle between the vehicle heading and the X - axis. From the positive X - axis counter - clockwise to the negative X - axis is (0, π), and from the positive X - axis clockwise to the negative X - axis is (0, - π).

[0084] Step S12, establish a vehicle coordinate system with the initial position coordinates, convert the end position coordinates based on the vehicle coordinate system and perform normalization to obtain the converted initial position coordinates and end position coordinates. As Figure 4 shown, the specific content is as follows:

[0085] Establish a vehicle coordinate system. Taking the current vehicle coordinate point P(x, y, θ) as the coordinate origin and the θ direction as the positive x - axis direction, establish a vehicle coordinate system xAy. Then the position coordinates of the initial position A in the vehicle coordinate system xAy are (0, 0, 0). At the same time, the position coordinates of the end position B in the current vehicle coordinate system xAy, that is:

[0086]

[0087] The coordinate conversion formula of the end position B in the current vehicle coordinate system xAy is:

[0088]

[0089] Normalize the end position B with a turning radius R min , and R min = 1, that is:

[0090]

[0091] Step S13, perform path planning based on the converted initial position coordinates and end position coordinates to obtain path coordinate information, the specific content is as follows:

[0092] The industrial control computer 1 performs path planning according to the calculated coordinates of the initial position A and the end position B, and selects a group with the shortest total path length L. The optimal solution is the radian value of each section of the path; the path selection and the calculation of the radian value of each section of the path are completed.

[0093] The Reeds-Shepp path planning method can perform path planning for 9 major path forms. Each form of path can be combined with 6 different motion modes, so 48 forms of path forms can be obtained, which can be expressed as:

[0094]

[0095] In the formula, C is a left turn or a right turn; S is a straight line; | means the vehicle changes from forward to backward or from backward to forward; β is the radian value passed by this section of the turning path; The radian value passed by this section of the turning path is L + is the vehicle turning left and moving forward; L - is the vehicle turning left and moving backward; R + is the vehicle turning right and moving forward; R - is the vehicle turning right and moving backward; S + is the vehicle moving straight forward; S - is the vehicle moving straight backward;

[0096] Solve for L + R - L + type, L + R - L - type path, which can be expressed as:

[0097] (a 1 , θ v ′ n ) = R(x vn - sinθ vn , y vn - 1 + cosθ vn )

[0098]

[0099] In the formula, the function R converts the Cartesian coordinate values within the brackets into polar coordinate values; a, b, and c are the radian values of three circular arc paths; L represents the total length of the path, where if L = ∞, it means there is no solution for the path.

[0100] Among them, the R function can convert Cartesian coordinate values into polar coordinate values, expressed as:

[0101]

[0102] Among them, a, b, and c are expressed as:

[0103]

[0104] Among them, the M function can convert the radian value to (-π, π], expressed as:

[0105]

[0106] Solve for L + S + L + The S-shaped path can be expressed as:

[0107] (a, b) = R(x vn - sinθ vn , y vn - 1 + cosθ vn )

[0108]

[0109] Among them, c is expressed as:

[0110] c = M(θ vn - b)

[0111] Solve for L + S + R + The R-shaped path can be expressed as:

[0112] (a 1 , b 1 ) = R(x vn + sinθ vn , y vn - 1 - cosθ vn )

[0113]

[0114] Among them, a, b, and c are expressed as:

[0115]

[0116] Solve for L + R β + L β - R - Type of path, can be expressed as:

[0117]

[0118] (b, c) = τω(a, -a, x vn +sinθ vn , y vn -1 - cosθ vn , θ vn )

[0119]

[0120] In the formula, the radian value corresponding to the first - stage path is b; the radian values corresponding to the second - stage and third - stage paths are a, -a; the radian value corresponding to the fourth - stage path is c;

[0121] Among them, the function τω is expressed as:

[0122] (τ, ω) = τω(a, c, ζ, η, θ vn )

[0123]

[0124] Among them, the parameter b 1 、b 2 、ζ、η are expressed as:

[0125]

[0126] Solve for L + R β - L β - R + Type of path, can be expressed as:

[0127] (b, c) = τω(a, a, x vn +sinθ vn , y vn -1 - cosθ vn , θ vn )

[0128]

[0129] Among them, a can be expressed as:

[0130] a = -arccosρ

[0131] Solve type path, which can be expressed as:

[0132] (ρ,θ v ′ n ) = M(x vn - sinθ vn , y vn - 1 + cosθ vn )

[0133]

[0134] where a, b, and c can be expressed as:

[0135]

[0136] Solve type path, which can be expressed as:

[0137] (ρ,θ v ′ n ) = M(-y + 1 + cosθ vn , x + sinθ vn )

[0138]

[0139] where a, b, and c can be expressed as:

[0140]

[0141] Solve type path, which can be expressed as:

[0142] (ρ,θ v ′ n ) = M(x vn + sinθ vn , y vn - 1 - cosθ vn ,)

[0143]

[0144] where a, b, and c can be expressed as:

[0145]

[0146] Calculate the planned path points. After obtaining the radian values of each section of the path, the path points on each section of the circular arc and straight-line path can be calculated by solving the circular arc equation and the straight-line equation. Combining all the path points in order gives the total path. Taking the left-turn forward path point L 1 which can be expressed as:

[0147]

[0148] In the formula, θ Δ is the heading angle increment between each path point;

[0149] The straight-ahead path point S 1 can be expressed as:

[0150]

[0151] In the formula, l Δ is the path point spacing increment;

[0152] The right-turn-ahead path point R 1 can be expressed as:

[0153]

[0154] The left-turn-backward path point L 2 can be expressed as:

[0155]

[0156] The straight-backward path point S 2 can be expressed as:

[0157]

[0158] The right-turn-backward path point R 2 can be expressed as:

[0159]

[0160] Through four path conversion methods of time reversal, mapping, time reversal + mapping, and backward transformation, the remaining 39 paths can be further solved. Among them, time reversal can be expressed as:

[0161]

[0162] In the formula, i = 0...n; P 1 ...P n is the original path coordinate point; P 1 ′...P n ′ is the path coordinate point after time reversal;

[0163] Mapping can be expressed as:

[0164]

[0165] Time reversal + mapping can be expressed as:

[0166]

[0167] Backward transformation can be expressed as:

[0168]

[0169] Select the path with the minimum total length L of each path, and the optimal path solution is the radian value of each segment of the path.

[0170] Step S14, execute the adaptive safe path addition strategy according to the path coordinate information to obtain the optimal safe path coordinate information and send it to the vehicle controller. The specific content is as follows:

[0171] Add the adaptive safe path at the initial position, end position, and turning point position in the path according to the path coordinate information to obtain the first safe path point coordinate information, as Figure 5 shown. The specific process is as follows:

[0172] Add the adaptive safe path at the initial position, end position, and turning point position. According to the safety requirements for the use of articulated loaders, during actual operation, the front and rear vehicle bodies of the loader should form a straight line when shoveling and discharging materials. In addition, to maintain good path tracking control effect, the front and rear vehicle bodies should also form a straight line at the turning point. Therefore, the adaptive safe path is added in the planned path. The addition of the adaptive safe path l can be expressed as:

[0173]

[0174] In the formula, θ a is the current articulation angle of the vehicle;

[0175] The coordinates of each point for adding the adaptive safe path at the initial position are:

[0176]

[0177] The coordinates of each point for adding the adaptive safe path at the end position are:

[0178]

[0179] The coordinates of each point for adding the adaptive safe path at the turning point position are:

[0180]

[0181] Obtain the first distance from each point on the path to the boundary point through the first safe path point coordinate information; compare the first distances from multiple points on the path to the boundary point to obtain the first minimum distance D min1 :

[0182] Collect the boundary point coordinates through the lidar 3, calculate the distance from each point on the planned optimal path to the boundary point, and select the first minimum distance D min1, can be expressed as:

[0183]

[0184] Where x b is the abscissa of the boundary point; y b is the ordinate of the boundary point; k = 0...m; h = 0...mn.

[0185] Judge whether the first minimum distance is less than the safety boundary value L d :

[0186] Yes, execute the next step;

[0187] No, the first safety path coordinate information is sent to the vehicle controller as the optimal safety path coordinate information;

[0188] Modify the first safety path coordinate information to obtain the second safety path point coordinate information with an adaptive turning radius, as Figure 6 shown, the specific steps are as follows:

[0189] Based on the updated path added to the adaptive safety path, different paths are planned with a turning radius of 10 as the basis and 1 as the step size increased to 25, that is:

[0190] R = 10 + ε R ΔR

[0191] Where ε R = 0,...,15; ΔR = 1;

[0192] Obtain the second distances from the points on multiple paths to the boundary point through the second safety path point coordinate information, and compare the second distances from the points on multiple paths to the boundary point to obtain the second minimum distance D from the points on the path to the boundary point min2 :

[0193] Collect the boundary point coordinates through the lidar 3, calculate the distances from each point in the planned optimal path to the boundary point, and select the second minimum distance D min2 , can be expressed as:

[0194]

[0195] Judge whether the second minimum distance is less than the safety boundary value L d :

[0196] Yes, execute the next step;

[0197] No, the second safety path coordinate information is sent to the vehicle controller as the optimal safety path coordinate information;

[0198] Optimize the straight-line lengths of the first and last segments of the first safety path coordinate information to obtain the third safety path point position coordinate information, as Figure 7 shown. The specific content is as follows:

[0199] Gradually increase the length l 1 of the first segment path added to the adaptive safety path and the length l 2 of the last segment path by 0.5 m step by step until the set upper limit value of 6 m to perform path planning, that is:

[0200]

[0201] where ε l = 0,..., 12; Δl = 0.5.

[0202] Obtain the third distances from the points on multiple paths to the boundary points through the third safety path point position coordinate information; compare the third distances from the points on multiple paths to the boundary points to obtain the third minimum distance D min3 from the points on the path to the boundary points:

[0203] Collect the boundary point coordinates through the lidar 3, calculate the distances from each point on the planned optimal path to the boundary points, and select the third minimum distance D min3 , which can be expressed as:

[0204]

[0205] Judge whether the third minimum distance is less than the safety boundary value:

[0206] Yes, execute the next step;

[0207] No, send the third safety path coordinate information as the optimal safety path coordinate information to the vehicle controller;

[0208] Perform starting point allowable error correction on the first safety path coordinate information to obtain the fourth safety path point position coordinate information, as Figure 8 shown. The specific content is as follows:

[0209] On the basis of the updated path added to the adaptive safety path, perform starting point allowable error correction, and slightly adjust the heading angle θ 0 of the path planning starting point, and gradually increase or decrease it by as the step until the correction boundary , that is:

[0210] θ 0 ′ = θ 0 ±ε θ Δθ

[0211] where ε θ = 0,..., 7;

[0212] Obtain the fourth distances from the points on multiple paths to the boundary points through the fourth safety path point coordinate information; compare the fourth distances from the points on multiple paths to the boundary points to obtain the fourth minimum distance D from the points on the path to the boundary point min4 ,, which can be expressed as:

[0213]

[0214] Judge whether the fourth minimum distance D min4 is less than the safety boundary value:

[0215] Yes, give an error prompt and display that the current working space is narrow and a safe path cannot be planned. Please modify the safety boundary value and re-add the adaptive safety path at the initial position, end position, and turning point positions in the path according to the path coordinate information

[0216] No, send the fourth safety path coordinate information as the optimal safety path coordinate information to the vehicle controller

[0217] Step S20: The vehicle controller obtains the current vehicle speed and executes the path tracking control strategy based on Tube-MPC according to the optimal safety path coordinate information and the reference vehicle speed. The specific steps are as follows:

[0218] Judge whether the signal of the unmanned operation solenoid valve is powered on:

[0219] Yes, obtain the current vehicle speed and execute the path tracking control strategy based on Tube-MPC according to the optimal safety path coordinate information and the reference vehicle speed

[0220] No, the control system is operated by the driver

[0221] The vehicle controller calculates the current vehicle speed according to the collected rotational speeds of each wheel hub motor. The calculation process of the current vehicle speed v is as follows:

[0222]

[0223] In the formula, ω fl , ω fr , ω rl , ω rr , are the rotational speeds of the left front wheel, right front wheel, left rear wheel, and right rear wheel respectively; r fl , r fr , r rl , r rr are the radii of the left front wheel, right front wheel, left rear wheel, and right rear wheel respectively; a b is the deceleration that the vehicle can achieve when braking on a general road surface. Here, a b takes the gravitational acceleration g; t is the braking time

[0224] The vehicle controller performs path tracking control based on Tube-MPC according to the point coordinate information x of the optimal safe path planned by the industrial control computer ref , y ref , θ ref and the preset reference vehicle speed v ref The path tracking control based on Tube-MPC is carried out as follows: The calculation process of the path tracking control strategy based on Tube-MPC is as follows:

[0225] Establish a loader kinematic model, considering that both the front and rear bodies of the loader are rigid bodies and the tires do not slip laterally, that is:

[0226]

[0227] In the formula, v f is the longitudinal speed of the front body; v r is the longitudinal speed of the rear body; L f is the distance from the front body axle to the hinge point; L r is the distance from the rear body axle to the hinge point; ω f is the heading angle of the front body; ω r is the heading angle of the rear body;

[0228] The above formula can be simplified, that is:

[0229]

[0230] The non-linear kinematic model of the loader is transformed into a state space equation, that is:

[0231]

[0232] Linearization processing. At a certain current moment k, taking the optimal path point at the current moment as the reference value (x ref , y ref , θ ref ) and substituting it into the non-linear kinematic model of the loader, and performing a first-order Taylor expansion at this point, we can get:

[0233]

[0234] In the formula, θ aref is the reference hinge angle at the current moment; v ref is the reference vehicle speed at the current moment;

[0235] Discretization processing. Discretize the linearized loader kinematic model, and the nominal system can be obtained as:

[0236]

[0237] In the formula, T is the sampling time;

[0238] Build the actual system. Considering that the kinematic model of the loader ignores external disturbances such as tire side slip, the actual system is built on the basis of the nominal system and can be expressed as:

[0239] x(k + 1) = Ax(k) + Bu(k) + w(k)

[0240] where w(k) is a bounded disturbance quantity and w(k) ∈ i 4 ; x(k) ∈ X, u(k) ∈ U, w(k) ∈ W are the constraints on the system, where X, U, and W are polyhedral sets containing interior points;

[0241] The Tube-control law can be expressed as:

[0242]

[0243] where K is the state feedback gain;

[0244] Establish the error system. The error between the actual system and the nominal system can be expressed as:

[0245]

[0246] Define K ∈ i 2×4 , and let A K @A n + B n K, then the closed-loop state equation of the error system is expressed as:

[0247] s(k + 1) = A K s(k) + w(k)

[0248] The robust positive invariant set (RPI) S of the error system is expressed as:

[0249]

[0250] Therefore, it satisfies:

[0251]

[0252] where ⊕ represents the Minkowski sum operation between sets.

[0253] Among them, the Minkowski sum operation and the Pontryagin difference operation between two sets can be defined as:

[0254]

[0255] The state quantity and control quantity of the nominal system can be expressed as:

[0256]

[0257] Solve for the minimum robust positive invariant set (mRPI) S min . The outer approximation S(α, s) of the robust positive invariant set (RPI) S can be expressed as:

[0258]

[0259] where s is the number of iterations, s ∈ ￥ + ; α is a scalar, and α ∈ [0, 1);

[0260] Each time the robust positive invariant set is obtained online, the number of iterations s is increased, and there is no need for an explicit representation of the robust positive invariant set, and the scalar α is small, so it can be further simplified, that is:

[0261]

[0262] Then for the disturbance set at z ∈ R n the function is defined as:

[0263]

[0264] To enable the polyhedron to contain the influence brought by the disturbance set W, the nominal system X is constrained as:

[0265]

[0266] where f i ∈ i 4 、g i ∈ i;

[0267] The polyhedron disturbance set W is calculated as:

[0268]

[0269] When the disturbance set W is in the form of a norm ball, fast calculation can be carried out, that is:

[0270]

[0271] Tube-MPC optimization solution, that is:

[0272]

[0273] where N is the prediction horizon; Q, R, P are the state, control, and terminal cost weight matrices respectively; is the reference state of the nominal system;

[0274] Step S30: The industrial control computer calculates the relative distance based on the real-time positioning information of the lidar and the end position set on the optimal safe path. When the distance between the real-time positioning information and the end position is less than or equal to the preset end stop distance, the control process ends. The specific steps are as follows:

[0275] The industrial control computer calculates the relative distance D based on the real-time positioning information of the lidar and the end position set on the optimal safe path ref ; The relative distance D between the current position and the end position ref The calculation process is as follows:

[0276] Based on the current vehicle position coordinates (x, y, θ) and the end position B coordinates (x n , y n , θ n ), calculate the relative distance D ref , that is:

[0277]

[0278] Compare the relative distance D between the current position and the end position ref with the preset end stop distance D final . If it is greater than the preset end stop distance D final , continue to execute the path tracking control strategy based on Tube-MPC; if it is less than or equal to the preset end stop distance D final , then exit this round of tracking control, and the end point has been reached.

[0279] The third embodiment of the present invention provides a wheel loader on the basis of the first embodiment, including the path tracking control system of the first embodiment.

[0280] Although the embodiments of the present invention have been disclosed as above, they are not limited to the applications listed in the specification and embodiments. It can be fully applied to various fields suitable for the present invention. For those familiar with the field, additional modifications can be easily made. Therefore, without departing from the general concept defined by the claims and the equivalent scope, the present invention is not limited to the specific details and the illustrated examples here.

Claims

1. A path tracking control system, characterized in that: It includes an industrial computer, which is electrically connected to the laser radar and the vehicle controller respectively. The vehicle controller is electrically connected to the articulation angle sensor, the steering oil pump oil circuit pressure sensor, the servo steering oil pump oil circuit pressure sensor, the rod cavity pressure sensor, the rodless cavity pressure sensor, the solenoid reversing valve, the unmanned operation solenoid valve, the solenoid proportional reversing valve and the motor controller respectively. The motor controller is electrically connected to the wheel hub motor.

2. The path tracking control system according to claim 1, characterized in that: The motor controller includes a front body left motor controller, a front body right motor controller, a rear body left motor controller and a rear body right motor controller, and the wheel hub motor includes: The left wheel hub motor of the front vehicle body is electrically connected to the left motor controller of the front vehicle body; The front right wheel hub motor is electrically connected to the front right motor controller; The left wheel hub motor of the rear vehicle body is electrically connected to the left motor controller of the rear vehicle body; The wheel hub motor on the right side of the rear body is electrically connected to the motor controller on the right side of the rear body.

3. The path tracking control system according to claim 2, characterized in that: The electromagnetic reversing valve is respectively connected to the servo steering oil pump, the priority valve and the unmanned operation electromagnetic valve; the servo steering oil pump is respectively connected to the servo steering oil pump oil filter, the servo steering oil pump oil circuit pressure sensor and the overflow valve; the unmanned operation electromagnetic valve is respectively connected to the electromagnetic proportional reversing valve; the electromagnetic proportional reversing valve, the priority valve, the overflow valve and the servo steering oil pump oil filter are all connected to the manual driving hydraulic steering system.

4. The path tracking control system according to claim 3, characterized in that: The manually driven hydraulic steering system includes a left steering hydraulic cylinder, a right steering hydraulic cylinder and a hydraulic oil tank which are respectively connected to the electromagnetic proportional reversing valve, the rod chambers of the left steering hydraulic cylinder and the right steering hydraulic cylinder are respectively connected to the rod chamber pressure sensors, the rodless chambers of the left steering hydraulic cylinder and the right steering hydraulic cylinder are respectively connected to the rodless chamber pressure sensors, the left steering hydraulic cylinder, the right steering hydraulic cylinder and the hydraulic oil tank are respectively connected to the steering gear, the steering gear is connected to the unmanned operation solenoid valve, the hydraulic oil tank is respectively connected to the overflow valve, the steering oil pump oil filter, the servo steering oil pump oil filter and the priority valve, the steering oil pump oil filter is connected to the steering oil pump, and the steering oil pump is respectively connected to the priority valve, the overflow valve and the steering oil pump oil circuit pressure sensor.

5. A path tracking control method, characterized in that: The path tracking control system as claimed in any one of claims 1 to 4 comprises: The industrial computer obtains the initial position and the terminal position of the loader operation, performs normalization setting of the wheel loader posture to obtain the converted initial position coordinates and the terminal position coordinates, executes the path planning strategy according to the converted initial position coordinates and the terminal position coordinates to obtain the optimal safe path coordinate information and sends it to the vehicle controller; The vehicle controller obtains the current vehicle speed and executes a path tracking control strategy based on Tube-MPC according to the optimal safety path coordinate information and the reference vehicle speed; The industrial computer calculates the relative distance based on the real-time positioning information of the laser radar and the end position set on the optimal safety path. When the distance between the real-time positioning information and the end position is less than or equal to the preset end stop distance, the control process ends.

6. The path tracking control method according to claim 5, characterized in that: The step of obtaining the initial position and the terminal position of the loader operation and performing normalization setting of the wheel loader posture includes: Set the initial position coordinates and the end position coordinates in the geodetic coordinate system; A vehicle coordinate system is established with the initial position coordinates, and the terminal position coordinates are transformed and normalized based on the vehicle coordinate system to obtain the transformed initial position coordinates and terminal position coordinates.

7. The path tracking control method according to claim 6, characterized in that: The executing of the path planning strategy according to the converted initial position coordinates and the terminal position coordinates to obtain the optimal safe path coordinate information and sending it to the vehicle controller includes: Performing path planning according to the converted initial position coordinates and end point position coordinates to obtain path coordinate information; An adaptive safety path adding strategy is executed according to the path coordinate information to obtain the optimal safety path coordinate information and send it to the vehicle controller.

8. The path tracking control method according to claim 7, characterized in that: The step of executing the adaptive safety path adding strategy according to the path coordinate information to obtain the optimal safety path coordinate information and sending it to the vehicle controller includes: Adding an adaptive safety path at the initial position, the end position and the turning point position in the path according to the path coordinate information to obtain the first safety path point coordinate information; Obtaining first distances from multiple points on the path to boundary points through the first safe path point coordinate information; Comparing a plurality of first distances to the boundary points on the path to obtain a first minimum distance from a point on the path to the boundary point; Determine whether the first minimum distance is less than the safety boundary value: Yes, proceed to the next step; No, the first safe path coordinate information is the optimal safe path coordinate information sent to the vehicle controller; Performing adaptive turning radius modification on the first safety path coordinate information to obtain second safety path point coordinate information; Obtaining second distances from multiple points on the path to the boundary point through the coordinate information of the second safe path point; Comparing the second distances from a plurality of the paths to the boundary points to obtain a second minimum distance from a point on the path to the boundary point; Determine whether the second minimum distance is less than the safety boundary value: Yes, proceed to the next step; No, the second safe path coordinate information is the optimal safe path coordinate information sent to the vehicle controller; Optimizing the length of the first and last straight lines of the first safety path coordinate information to obtain the third safety path point coordinate information; Obtaining third distances from multiple points on the path to the boundary point through the third safe path point coordinate information; Comparing the third distances from a plurality of the paths to the boundary points to obtain a third minimum distance from a point on the path to the boundary point; Determine whether the third minimum distance is less than the safety boundary value: Yes, proceed to the next step; No, the third safe path coordinate information is the optimal safe path coordinate information sent to the vehicle controller; Performing starting point allowable error correction on the first safety path coordinate information to obtain fourth safety path point coordinate information; Obtaining fourth distances from multiple points on the path to the boundary point through the fourth safe path point coordinate information; Comparing a plurality of fourth distances from the path to the boundary point to obtain a fourth minimum distance from a point on the path to the boundary point; Determine whether the fourth minimum distance is less than the safety boundary value: If yes, an error message will be displayed and it will be shown that the current working space is too small to plan a safe path. Please modify the safety boundary value and add an adaptive safe path at the initial position, end position and turning point position of the path according to the path coordinate information. No, the fourth safe path coordinate information is the optimal safe path coordinate information sent to the vehicle controller.

9. The path tracking control method according to claim 8, characterized in that: Before the vehicle controller obtains the current vehicle speed and executes the path tracking control strategy based on Tube-MPC according to the optimal safety path coordinate information and the reference vehicle speed, it also includes: Determine whether the unmanned operation solenoid valve signal is energized: Yes, obtain the current vehicle speed and execute the path tracking control strategy based on Tube-MPC according to the optimal safe path coordinate information and the reference vehicle speed; No, the control system is operated by the driver.

10. A wheel loader, characterized in that: A path tracking control system comprising the path tracking control system according to any one of claims 1 to 4.

Images