Loader path tracking control method and related device

Abstract

The invention discloses a loader path tracking control method and a related device, a preview algorithm is adopted to predict the state and the position of a loader at the next moment, the transverse error, the longitudinal error and the course error at the next moment are calculated in combination with a preset planning path, and under the condition that the transverse error exists at the next moment, the position of the loader is tracked. According to the transverse error of the current moment, the LQR controller is adopted to achieve transverse control of the loader, under the condition that the longitudinal error exists at the next moment and the longitudinal error of the current moment is smaller than 0, the segment PID algorithm is adopted to achieve longitudinal control of the loader, and compared with an existing path tracking control method, the tracking precision is higher.

Description

Technical Field

[0001] The present invention relates to a path tracking control method and related device for a loader, belonging to the technical field of autonomous construction machinery control. Background Art

[0002] In the current engineering construction field, the application of unmanned electric loaders (referred to as "loaders" for short) is becoming increasingly widespread. However, the existing path tracking control of loaders is mostly based on a model-free PID algorithm, resulting in low path tracking accuracy under complex working conditions, which easily causes deviations in the driving process of the loader and affects the operation efficiency and safety. Summary of the Invention

[0003] The present invention provides a path tracking control method and related device for a loader, which solves the problems disclosed in the background art.

[0004] According to one aspect of the present disclosure, there is provided a path tracking control method for a loader, including:

[0005] Obtain the state information and position information of the loader at the current moment;

[0006] According to the state information and position information at the current moment, use the preview algorithm to predict the position information of the loader at the next moment, and convert the position information at the next moment into the position information in the Frenet coordinate system;

[0007] According to the position information in the Frenet coordinate system and the preset planned path, calculate the lateral error and longitudinal error of the position of the loader at the next moment compared with the planned path; wherein, the lateral error and longitudinal error at the next moment are respectively used as the lateral error and longitudinal error at the current moment in the path tracking control method of the loader at the next moment;

[0008] If there is a lateral error at the next moment, use the LQR controller to perform lateral control of the loader according to the lateral error at the current moment, the state information at the current moment and the planned path;

[0009] If there is a longitudinal error at the next moment and the longitudinal error at the current moment is less than 0, use the segmented PID algorithm to perform longitudinal control of the loader according to the range of the longitudinal error at the current moment, the state information at the current moment and the planned path.

[0010] Further, converting the position information into the information in the Frenet coordinate system includes:

[0011] Convert the position information into the information in the ECEF coordinate system;

[0012] Convert the information in the ECEF coordinate system into the information in the ENU coordinate system;

[0013] Convert the information in the ENU coordinate system to the information in the Frenet coordinate system.

[0014] Furthermore, the lateral control of the loader is to control the steering angle of the loader.

[0015] According to the lateral error at the current moment, the state information at the current moment, and the planned path, an LQR controller is used to perform the lateral control of the loader, including:

[0016] According to the lateral error at the current moment, the state information at the current moment, and the planned path, an LQR controller is used to obtain the optimal steering angle at the next moment, and according to the optimal steering angle at the next moment, the steering angle control of the loader is performed.

[0017] Furthermore, the LQR controller is constructed based on the kinematic model of the loader.

[0018] The LQR controller is constructed based on the kinematic model of the loader.

[0019] The kinematic model of the loader is:

[0020] X(k + 1)=AX(k)+Bu(k);

[0021] In the formula, the coefficient matrix , the coefficient matrix , the state matrix at the (k + 1)th moment , the state matrix at the kth moment , the control quantity at the kth moment , L1 and L2 are the distances from the hinge of the loader to the front axle and from the hinge of the loader to the rear axle respectively, v p is the resultant velocity of the front axle of the loader, are the derivatives of the lateral error and the heading error at the kth moment respectively, is the steering angular velocity of the hinge of the loader at the kth moment, is the steering angle of the hinge of the loader at the kth moment, are the lateral error and the heading error at the kth moment respectively, and the heading error is obtained by calculating the heading angle in the state information at the current moment and the heading angle of the corresponding point in the planned path;

[0022] The objective function of the LQR controller is:

[0023] ;

[0024] In the formula, J is the linear quadratic performance index of the LQR controller, N is the total number of discrete time points, X and u represent X(k + 1) and u(k) in the kinematic model formula of the loader respectively, the superscript T represents the transpose, and Q and R are both diagonal matrices.

[0025] Further, the longitudinal control of the loader is to control the speed of the loader;

[0026] According to the range of the longitudinal error at the current moment, the state information at the current moment, and the planned path, a piecewise PID algorithm is adopted to perform the longitudinal control of the loader, including:

[0027] If the longitudinal error at the current moment is greater than the first negative threshold, the second threshold is used as the proportional gain, and according to the speed in the state information at the current moment and the speed corresponding to the path point at the next moment in the planned path, a piecewise PID algorithm is adopted to control the speed of the loader;

[0028] If the longitudinal error at the current moment is less than the first negative threshold, the third threshold is used as the proportional gain, and according to the speed in the state information at the current moment and the speed corresponding to the path point at the next moment in the planned path, a piecewise PID algorithm is adopted to control the speed of the loader.

[0029] Further, it also includes that if there is a longitudinal error at the next moment and the longitudinal error at the current moment is greater than 0, the longitudinal control of the loader is performed according to the range of the longitudinal error at the current moment.

[0030] Further, the longitudinal control of the loader is to control the speed of the loader;

[0031] According to the range of the longitudinal error at the current moment, the longitudinal control of the loader is performed, including:

[0032] If the longitudinal error at the current moment is less than the first positive threshold, a neutral coasting strategy is adopted to control the kinetic energy recovery and deceleration of the loader;

[0033] If the longitudinal error at the current moment is not less than the first positive threshold, the loader is controlled to brake.

[0034] According to another aspect of the present disclosure, a loader path tracking control device is provided, including:

[0035] An acquisition module that acquires the state information and position information of the loader at the current moment;

[0036] A preview conversion module that, according to the state information and position information at the current moment, adopts a preview algorithm to predict the position information of the loader at the next moment, and converts the position information at the next moment into the position information in the Frenet coordinate system;

[0037] An error calculation module that calculates the lateral error and longitudinal error of the position of the loader at the next moment compared with the planned path according to the position information in the Frenet coordinate system and the preset planned path; wherein, the lateral error and longitudinal error at the next moment are respectively used as the lateral error and longitudinal error at the current moment in the loader path tracking control method at the next moment;

[0038] The lateral control module, if there is a lateral error at the next moment, according to the lateral error at the current moment, the state information at the current moment, and the planned path, adopts an LQR controller to perform the lateral control of the loader;

[0039] The longitudinal control module, if there is a longitudinal error at the next moment and the longitudinal error at the current moment is less than 0, according to the range of the longitudinal error at the current moment, the state information at the current moment, and the planned path, adopts a piecewise PID algorithm to perform the longitudinal control of the loader.

[0040] According to another aspect of the present disclosure, there is provided a computer-readable storage medium storing one or more programs, the one or more programs including instructions that, when executed by a computing device, cause the computing device to execute the loader path tracking control method.

[0041] According to another aspect of the present disclosure, there is provided a computer device including one or more processors and one or more memories, the one or more programs being stored in the one or more memories and configured to be executed by the one or more processors, the one or more programs including instructions for executing the loader path tracking control method.

[0042] The beneficial effects achieved by the present invention: The present invention uses a preview algorithm to predict the state and position of the loader at the next moment, combines the preset planned path, calculates the lateral error, longitudinal error, and heading error at the next moment. When there is a lateral error at the next moment, according to the lateral error at the current moment, an LQR controller is adopted to achieve the lateral control of the loader. When there is a longitudinal error at the next moment and the longitudinal error at the current moment is less than 0, a piecewise PID algorithm is adopted to achieve the longitudinal control of the loader. Compared with the existing path tracking control methods, the tracking accuracy is higher. Description of the Drawings

[0043] Figure 1 It is a flowchart of the loader path tracking control method;

[0044] Figure 2 It is a schematic diagram of the kinematic model of a three-degree-of-freedom articulated vehicle;

[0045] Figure 3 It is an effect diagram of the tracking of the existing method;

[0046] Figure 4 It is an effect diagram of the tracking of the loader path tracking control method;

[0047] Figure 5 It is an effect diagram of controlling the vehicle error of the existing method;

[0048] Figure 6 It is an effect diagram of controlling the vehicle error of the loader path tracking control method;

[0049] Figure 7 It is a block diagram of a path tracking control device for a loader. Specific implementation manners

[0050] Next, the technical solutions in the embodiments of the present disclosure will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present disclosure. Obviously, the described embodiments are only a part rather than all of the embodiments of the present disclosure. The following description of at least one exemplary embodiment is actually only illustrative and in no way limits the present disclosure and its application or use. Based on the embodiments in the present disclosure, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the scope of protection of the present disclosure.

[0051] Unless otherwise specifically stated, the relative arrangements, numerical expressions and values of the components and steps set forth in these embodiments do not limit the scope of the present disclosure.

[0052] Meanwhile, it should be understood that, for the sake of description, the dimensions of the various parts shown in the accompanying drawings are not drawn in actual proportional relationship.

[0053] For technologies, methods and devices known to those of ordinary skill in the relevant art, they may not be discussed in detail, but where appropriate, the said technologies, methods and devices should be regarded as a part of the description.

[0054] In all the examples shown and discussed here, any specific value should be construed as merely exemplary and not as a limitation. Therefore, other examples of the exemplary embodiments may have different values.

[0055] It should be noted that: similar symbols and letters represent similar items in the following accompanying drawings. Therefore, once an item is defined in one drawing, it does not need to be further discussed in subsequent drawings.

[0056] To solve the problem of low tracking accuracy in existing path tracking control methods, the present disclosure proposes a path tracking control method for a loader. Specifically, an LQR controller is designed based on a kinematic model, and a preview algorithm, an LQR controller, and a segmented PID algorithm are used to achieve the lateral control and longitudinal control of the loader. This method can be executed by a control device, which can be a terminal device or a server. Among them, the terminal device can include, but is not limited to, mobile phones, computers, smart wearable devices, smart vehicle-mounted devices, etc., and the embodiments of the present application do not make limitations; the server can be an independent physical server, or a server cluster or distributed system composed of multiple physical servers, or a cloud server that provides basic cloud computing services such as cloud services, cloud databases, cloud computing, big data, and artificial intelligence platforms, etc., and the embodiments of the present application do not make limitations. Optionally, this method can also be executed collaboratively by multiple electronic devices with computing power. For the convenience of description, the subsequent embodiments will be described with the execution by a control device.

[0057] See Figure 1 , Figure 1 is a flowchart of a path tracking control method for a loader provided by an embodiment of the present disclosure. This method can be executed by a control device, and this method can at least include the following steps:

[0058] Step 1, obtain the state information and position information of the loader at the current moment.

[0059] It should be noted that the state information at least includes information such as the speed, acceleration, heading angle, and articulation angle of the loader, and these information can be obtained through an IMU (inertial measurement unit) and an articulation angle sensor, etc. The position information can be obtained through a navigation and positioning device, such as an RTK (real-time kinematic) device. The obtained position information is in the WGS-84 geodetic coordinate system, which is a geographic coordinate system for positioning on the earth's surface, and represents the position with longitude, latitude, and altitude. Among them, the units of longitude and latitude are radians, and the unit of altitude is meters.

[0060] Step 2, according to the state information and position information at the current moment, use the preview algorithm to predict the position information of the loader at the next moment, and convert the position information at the next moment into the position information in the Frenet coordinate system.

[0061] It should be noted that in order to make the loader fit the planned path with as small a control amount as possible in subsequent control, and further reduce the lag of lateral control, that is, to achieve early control and improve stability, the preview algorithm is used to predict the position information of the next step. The formula of the preview algorithm can be expressed as:

[0062] ;

[0063] ;

[0064] Of course, in order to compare with the actual state collected at the next moment, the state information can also be processed by a preview algorithm. The formula is as follows:

[0065] ;

[0066] ;

[0067] ;

[0068] ;

[0069] Define k to represent the current moment and k + 1 to represent the next moment. In the formula, is the predicted X-axis coordinate of the loader at the moment k + 1, and x is the X-axis coordinate of the loader at the moment k. is the predicted Y-axis coordinate of the loader at the moment k + 1, and y is the Y-axis coordinate of the loader at the moment k. is the predicted steering angle at the hinge of the loader at the moment k + 1. is the steering angle at the hinge of the loader at the moment k. is the steering angular velocity at the hinge of the loader at the moment k. is the predicted steering angular velocity at the hinge of the loader at the moment k + 1. is the predicted X-axis direction speed of the loader at the moment k + 1. is the predicted Y-axis direction speed of the loader at the moment k + 1, v p is the resultant speed of the front axle of the loader, t s is the predicted time step. is the reference heading angle of the planned path. is the heading angle of the front frame of the loader.

[0070] It should be noted that in order to facilitate tracking control, the position information needs to be converted into information in the Frenet coordinate system. The specific process can be as follows:

[0071] 1) Convert the position information into information in the ECEF coordinate system.

[0072] It should be noted that the ECEF coordinate system (i.e., the space rectangular coordinate system) has the center of the earth as the origin, the Z-axis points to the north pole of the earth, the X-axis points to the intersection of the prime meridian and the equator, and the Y-axis forms a right-handed coordinate system with the Z-axis and the X-axis.

[0073] The formula for converting WGS-84 geodetic coordinates to ECEF coordinates can be expressed as:

[0074] ;

[0075] ;

[0076] ;

[0077] In the formula, represents the longitude in the WGS-84 geodetic coordinate, represents the latitude in the WGS-84 geodetic coordinate, h represents the altitude in the WGS-84 geodetic coordinate, respectively represent the X-axis, Y-axis and Z-axis coordinates in the ECEF coordinate, represents the radius of the prime vertical, a = 6378137.0m represents the semi-major axis of the earth ellipsoid, represents the first eccentricity, f = 1 / 298.25722356 is the flattening of the earth.

[0078] 2) Convert the information in the ECEF coordinate system to the information in the ENU coordinate system.

[0079] It should be noted that since the lidar and inertial measurement unit collect data centered on themselves and measure the surrounding environment information in the directions of east, north, and up. Therefore, it is necessary to convert the ECEF coordinates to the local Cartesian (ENU) coordinates to fuse the position information and the sensor information in the same coordinate system and provide a unified data basis for subsequent tasks.

[0080] The process of converting the ECEF coordinates to the ENU coordinates can be as follows:

[0081] Assume the space rectangular coordinates (X0, Y0, Z0) of the reference point P0, and calculate the coordinate difference of the target point P(X1, Y1, Z1) relative to the reference point P0 as:

[0082] ΔX = X1 - X0;

[0083] ΔY = Y1 - Y0;

[0084] ΔZ = Z1 - Z0;

[0085] Construct the unit vectors of the three axes of the ENU coordinate system in the ECEF, which can be expressed as:

[0086] The east direction is along the tangent of the latitude line to the east, and the unit vector E is:

[0087] ;

[0088] The north direction is along the tangent of the meridian line to the north, and the unit vector N is:

[0089] ;

[0090] The sky direction is perpendicular to the ground and upward, and the unit vector U is:

[0091] ;

[0092] The constructed rotation matrix R is:

[0093] ;

[0094] Multiply the coordinate difference by the rotation matrix R to obtain the ENU coordinates. The formula can be expressed as:

[0095] ;

[0096] It can be obtained that:

[0097] .

[0098] 3) Convert the information in the ENU coordinate system to the information in the Frenet coordinate system.

[0099] It should be noted that in the actual traffic scenario, the planned path is complex and diverse, including various shapes such as curves and broken lines. In the ENU coordinate system, it is difficult to describe and process these complex paths, and the computational complexity is large. The Frenet coordinate system accurately describes the relative position relationship between the vehicle and the path through s (representing the longitudinal displacement along the road, that is, the curved distance from the road starting point to the current vehicle position) and d (representing the longitudinal displacement along the road, that is, the curved distance from the road starting point to the current vehicle position), and can better adapt to the representation and processing of complex paths. Therefore, it is necessary to further convert the information in the ENU coordinate system to the information in the Frenet coordinate system.

[0100] It should be noted that the above preview algorithm can be carried out in any coordinate system. For example, prediction can be carried out in the WGS-84 geodetic coordinate system, the ECEF coordinate system or the ENU coordinate system, and then further conversion can be carried out. For example, prediction can also be carried out in the Frenet coordinate system, that is, prediction is carried out after conversion.

[0101] It should be noted that the path tracking of the loader is to track the planned path. Specifically, it is to control the actual path of the loader to be as consistent with the planned path as possible. The planned path here can be planned by the hybrid A* algorithm. The planned path consists of a series of path points, and each point contains information such as speed, acceleration, coordinates, heading angle and curvature. In order to facilitate the comparison between the planned path and the actual path, like the position information of the loader, it is necessary to pre-convert the path points in the planned path into path points in the Frenet coordinate system.

[0102] Step 3: Calculate the lateral error and longitudinal error of the loader's next moment position compared with the planned path according to the position information in the Frenet coordinate system and the preset planned path; where the lateral error and longitudinal error of the next moment are respectively used as the lateral error and longitudinal error at the current moment in the loader path tracking control method at the next moment.

[0103] It should be noted that in the Frenet coordinate system, the position information mainly includes the accumulated arc length (i.e., longitudinal displacement) and the lateral displacement from the reference line. Define the current position of the loader as D1, and the path point closest to D1 in the planned path as D2. Here, when calculating the error, mainly compare the position information of point D1 in the Frenet coordinate system with the position information of point D2. Calculate the longitudinal error by subtracting the s-axis coordinates of the two, and calculate the lateral error by subtracting the d-axis coordinates of the two.

[0104] Step 4: If there is a lateral error at the next moment, use an LQR controller to perform lateral control of the loader according to the lateral error at the current moment, the state information at the current moment, and the planned path; if there is a longitudinal error at the next moment and the longitudinal error at the current moment is less than 0, use a segmented PID algorithm to perform longitudinal control of the loader according to the range of the longitudinal error at the current moment, the state information at the current moment, and the planned path.

[0105] It should be noted that if there is a lateral error, it indicates that there is a certain distance between the actual path and the planned path in the lateral direction, and the heading angle needs to be adjusted. That is, the lateral control of the loader here is actually to control the steering angle of the loader.

[0106] According to the lateral error at the current moment, the state information at the current moment, and the planned path, an LQR controller can be used to obtain the optimal steering angle at the next moment, and the loader steering angle control is performed according to the optimal steering angle at the next moment.

[0107] It should be noted that the LQR controller here is designed according to the kinematic model. In order for the loader to adapt to different working environments, the kinematic model here adopts a three-degree-of-freedom articulated vehicle kinematic model. See Figure 2 , since the loader is an articulated vehicle, the front and rear frames are connected by an active rotating joint, and the steering is achieved by changing the angle between the frames. The established kinematic model is derived based on geometric relationships and speed constraints. The state variables include lateral error, heading error (the deviation between the actual heading of the vehicle and the desired heading), and steering angle (the articulated angle between the front and rear frames).

[0108] The three-degree-of-freedom articulated vehicle kinematic model can be expressed as:

[0109] ;

[0110] ;

[0111] ;

[0112] In the formula, and are the derivatives of the longitudinal velocity and the lateral velocity at the midpoint of the front axle, is 's derivative, and L1 and L2 are the distances from the hinge point of the loader to the front axle and the distance from the hinge point of the loader to the rear axle, respectively.

[0113] The error equation of the model can be expressed as:

[0114] ;

[0115] In the formula, are the longitudinal error, lateral error, and heading error at time k, respectively, are the X-axis coordinate and Y-axis coordinate of the center point of the rear axle of the loader, respectively.

[0116] The state space equation of the model can be expressed as:

[0117] ;

[0118] In the formula, are the derivatives of respectively. Here, is calculated based on the heading angle in the current moment state information and the heading angle of the corresponding point (i.e., the nearest point) in the planned path, specifically by taking the difference.

[0119] Modify the kinematic model of the loader to:

[0120] X(k + 1)=AX(k)+Bu(k);

[0121] In the formula, the coefficient matrix , the coefficient matrix , the state matrix at time k + 1 , the state matrix at time k , the control quantity at time k , is the derivative of the heading error at time k.

[0122] The objective function of the LQR controller is the weighting of the accumulated tracking deviation and the accumulated control input during the tracking process. The formula can be expressed as:

[0123] ;

[0124] In the formula, J is the linear quadratic performance index of the LQR controller, N is the total number of discrete time points, representing the time span of the optimization problem, X and u represent X(k + 1) and u(k) in the kinematic model formula of the loader respectively, the superscript T represents transpose, and both Q and R are diagonal matrices. By controlling Q, X is made to approach the target state as closely as possible, and by controlling R, the amplitude of the control u is restricted to reduce energy consumption.

[0125] For the optimal solution of the objective function, the obtained optimal control quantity u is a linear function of X:

[0126] ;

[0127] In the formula, P is the solution of the following Riccati equation, , a larger Q matrix element means that it is desired that the tracking deviation can quickly approach zero, and a larger R matrix element means that it is desired that the control input can be as small as possible.

[0128] It should be noted that the longitudinal error is the difference between the actual trajectory and the arc length of the planned trajectory. If the difference is greater than 0, it means that the current position of the loader is ahead of the planned path point. If it is less than 0, it means that the current position of the loader is behind the planned path point. Different longitudinal control methods need to be adopted in different states.

[0129] When the longitudinal error at the next moment is less than 0, the piecewise PID algorithm can be further adopted according to the range of the longitudinal error at the current moment, the state information at the current moment, and the planned path for the longitudinal control of the loader.

[0130] If the distance between the current position and the planned path is far or the curvature of the planned path is small, a larger proportional gain coefficient can be used to increase the vehicle speed so that the loader can quickly reach the corresponding point on the planned path. If the distance between the current position and the planned path is close or the curvature of the planned path is large, a smaller proportional gain coefficient can be used to reduce the vehicle speed so that the loader can run smoothly. The specific process can be as follows:

[0131] If the longitudinal error at the current moment is greater than the first negative threshold, the second threshold is used as the proportional gain, and according to the speed in the state information at the current moment and the speed corresponding to the next moment path point in the planned path, the piecewise PID algorithm is adopted for the speed control of the loader; if the longitudinal error at the current moment is less than the first negative threshold, the third threshold is used as the proportional gain, and according to the speed in the state information at the current moment and the speed corresponding to the next moment path point in the planned path, the piecewise PID algorithm is adopted for the speed control of the loader; among them, the first negative threshold can be set to -2, the second threshold can be set to 10, and the third threshold can be set to 20.

[0132] It should be noted that the formula of the piecewise PID algorithm can be as follows:

[0133] ;

[0134] In the formula, h(t) is the control quantity of the piecewise PID algorithm, e is the error independent variable, K i is the fixed integral gain used to eliminate the steady-state error, K d is the fixed differential gain used to suppress overshoot, K P is the proportional gain, e(t) is the difference between the set value and the feedback value, is the integral independent variable, is the curvature;

[0135] ;

[0136] In the formula, are the highest proportional gain and the lowest proportional gain respectively, s is the arc length, s th is the arc length threshold, is the curvature threshold.

[0137] The speed is indirectly controlled by the PID output h(t):

[0138] ;

[0139] In the formula, are the speeds at time t and time t-1 respectively.

[0140] When the loader lags behind the planned path point, if the lag arc length is less than 2, set the proportional gain to 10 to make the loader accelerate gently. If the lag arc length is greater than 2, set the proportional gain to 20 to make the loader quickly move to the planned position.

[0141] When the longitudinal error at the next moment is greater than 0, the longitudinal control of the loader can be further carried out according to the range of the longitudinal error at the current moment. The specific process can be as follows:

[0142] If the longitudinal error at the current moment is less than the first positive threshold, adopt the neutral gear coasting strategy to control the kinetic energy recovery and deceleration of the loader. If the longitudinal error at the current moment is not less than the first positive threshold, control the loader to brake. Among them, the first positive threshold can be set to 1.

[0143] Specifically, when the loader is ahead of the planned path point, if the overrun arc length is less than 1, adopt the neutral gear coasting strategy to make the loader recover kinetic energy and decelerate. If the overrun arc length is greater than 1, make the loader brake.

[0144] To verify the effect of the above method, the above method is compared with the existing method, and the comparison Figure 3 and 4 (the coordinate unit in all figures is m). It can be seen that the path tracking effect of the above method is better, and the lateral error is controlled within 20 cm.

[0145] Comparison Figure 5 and 6 it can be seen that the path tracking error of the above method is lower, and the lateral displacement error is reduced by 62.5% compared with the existing method.

[0146] The above method uses a preview algorithm to predict the state and position of the loader at the next moment, combines the preset planned path, calculates the lateral error, longitudinal error and heading error at the next moment. When there is a lateral error at the next moment, an LQR controller is used according to the lateral error at the current moment to achieve the lateral control of the loader. When there is a longitudinal error at the next moment and the longitudinal error at the current moment is less than 0, a segmented PID algorithm is used to achieve the longitudinal control of the loader. Compared with the existing path tracking control method, the tracking accuracy is higher.

[0147] See Figure 7 , Figure 7 which is a block diagram of a loader path tracking control device provided by an embodiment of the present disclosure. Figure 7 The embodiment of is a virtual device that can be loaded and executed by a computer device, and the computer device may include the above control device. Figure 7 The device of may include an acquisition module, a preview conversion module, an error calculation module, a lateral control module and a longitudinal control module. When used to execute the above loader path tracking control method, it can:

[0148] The acquisition and conversion module acquires the state information and position information of the loader at the current moment.

[0149] The preview conversion module uses a preview algorithm to predict the position information of the loader at the next moment according to the state information and position information at the current moment, and converts the position information at the next moment into the position information in the Frenet coordinate system.

[0150] The error calculation module calculates the lateral error and longitudinal error of the loader's position at the next moment compared with the planned path according to the position information in the Frenet coordinate system and the preset planned path; wherein, the lateral error and longitudinal error at the next moment are respectively used as the lateral error and longitudinal error at the current moment in the loader path tracking control method at the next moment.

[0151] The lateral control module, if there is a lateral error at the next moment, uses an LQR controller according to the lateral error at the current moment, the state information at the current moment and the planned path to perform the lateral control of the loader.

[0152] The longitudinal control module, if there is a longitudinal error at the next moment and the longitudinal error at the current moment is less than 0, uses a segmented PID algorithm according to the range of the longitudinal error at the current moment, the state information at the current moment and the planned path to perform the longitudinal control of the loader.

[0153] The above device uses a preview algorithm to predict the state and position of the loader at the next moment, combines the preset planned path, calculates the lateral error, longitudinal error, and heading error at the next moment. When there is a lateral error at the next moment, according to the lateral error at the current moment, an LQR controller is used to achieve the lateral control of the loader. When there is a longitudinal error at the next moment and the longitudinal error at the current moment is less than 0, a segmental PID algorithm is used to achieve the longitudinal control of the loader. Compared with the existing path tracking control methods, the tracking accuracy is higher.

[0154] The present disclosure also relates to a computer-readable storage medium that stores one or more programs. The one or more programs include instructions that, when executed by a computing device, cause the computing device to execute the loader path tracking control method.

[0155] Based on the same technical solution, the present disclosure also relates to a computer device, including one or more processors and one or more memories. The one or more programs are stored in the one or more memories and are configured to be executed by the one or more processors. The one or more programs include instructions for executing the loader path tracking control method.

[0156] Those skilled in the art should understand that the embodiments of the present invention can be provided as a method, a system, or a computer program product. Therefore, the present invention can take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present invention can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk memories, CD-ROMs, optical memories, etc.) that contain computer-usable program code.

[0157] The present invention is described with reference to the flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to the embodiments of the present invention. It should be understood that each flow and / or block in the flowcharts and / or block diagrams, as well as the combination of flows and / or blocks in the flowcharts and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to the processors of general-purpose computers, special-purpose computers, embedded processors, or other programmable data processing devices to generate a machine, such that the instructions executed by the processors of the computer or other programmable data processing devices generate means for implementing the functions specified in Figure 1 one flow or multiple flows and / or blocks Figure 1 one block or multiple blocks.

[0158] These computer program instructions can also be stored in a computer-readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including instruction means embodying the functionality specified in the flowchart Figure 1 a flowchart or multiple flowcharts and / or blocks Figure 1 a block or multiple blocks.

[0159] These computer program instructions can also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer-implemented process, thereby providing steps for implementing the functionality specified in the flowchart Figure 1 a flowchart or multiple flowcharts and / or blocks Figure 1 a block or multiple blocks.

[0160] The above are only embodiments of the present invention and are not intended to limit the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principles of the present invention are included within the scope of the claims of the present invention pending approval of the application.

Claims

1. A loader path tracking control method, characterized in that: include: Get the current status and location information of the loader; According to the current state information and position information, the preview algorithm is used to predict the position information of the loader at the next moment, and the position information at the next moment is converted into the position information in the Frenet coordinate system; According to the position information in the Frenet coordinate system and the preset planned path, the lateral error and longitudinal error of the loader's position at the next moment compared with the planned path are calculated; wherein the lateral error and longitudinal error at the next moment are respectively used as the lateral error and longitudinal error at the current moment in the loader path tracking control method at the next moment; If there is a lateral error at the next moment, the LQR controller is used to perform lateral control of the loader according to the lateral error at the current moment, the state information at the current moment and the planned path; If there is a longitudinal error at the next moment and it is less than 0, the segmented PID algorithm is used to perform longitudinal control of the loader according to the range of the longitudinal error at the current moment, the current state information and the planned path.

2. The method according to claim 1, characterized in that Convert location information into Frenet coordinate system, including: Convert the position information into the ECEF coordinate system; Convert the information in the ECEF coordinate system to the information in the ENU coordinate system; Convert the information in the ENU coordinate system to the information in the Frenet coordinate system.

3. The method according to claim 1, characterized in that: The lateral control of the loader is to control the steering angle of the loader; According to the lateral error at the current moment, the current state information and the planned path, the LQR controller is used to perform lateral control of the loader, including: According to the lateral error at the current moment, the state information at the current moment and the planned path, the LQR controller is used to obtain the optimal steering angle at the next moment, and the steering angle of the loader is controlled according to the optimal steering angle at the next moment.

4. The method according to claim 1 or 3, characterized in that: The LQR controller is built based on the loader kinematic model; The kinematic model of the loader is: X(k+1)=AX(k)+Bu(k); In the formula, the coefficient matrix , the coefficient matrix , the state matrix at time k+1 , the state matrix at time k , the control quantity at time k , L1 and L2 are the distances from the loader articulation to the front axle and from the loader articulation to the rear axle, respectively, v p is the total speed of the loader's front axle, are the derivatives of the lateral error and heading error at time k, is the steering angular velocity of the loader articulation at time k, is the steering angle of the loader articulation at time k, are the lateral error and heading error at time k respectively. The heading error is calculated based on the heading angle in the current state information and the heading angle of the corresponding point in the planned path; The objective function of the LQR controller is: ； Where J is the linear quadratic performance index of the LQR controller, N is the total number of discrete time points, X and u represent X(k+1) and u(k) in the loader kinematic model formula, respectively, the superscript T represents the transpose, and Q and R are both diagonal matrices.

5. The method according to claim 1, characterized in that The loader longitudinal control is to control the speed of the loader; According to the range of the longitudinal error at the current moment, the current state information and the planned path, the segmented PID algorithm is used to perform the longitudinal control of the loader, including: If the longitudinal error at the current moment is greater than the first negative threshold, the second threshold is used as the proportional gain, and the loader speed is controlled by using the segmented PID algorithm according to the speed in the current state information and the speed corresponding to the path point at the next moment in the planned path; If the longitudinal error at the current moment is less than the first negative threshold, the third threshold is used as the proportional gain, and the segmented PID algorithm is used to control the loader speed according to the speed in the current state information and the speed corresponding to the path point at the next moment in the planned path.

6. The method according to claim 1, characterized in that It also includes if there is a longitudinal error at the next moment and it is greater than 0, performing longitudinal control of the loader according to the range of the longitudinal error at the current moment.

7. The method according to claim 6, characterized in that The loader longitudinal control is to control the speed of the loader; According to the range of the longitudinal error at the current moment, the loader longitudinal control is performed, including: If the longitudinal error at the current moment is less than the first positive threshold, the neutral gear coasting strategy is adopted to control the loader to recover energy and decelerate; If the longitudinal error at the current moment is not less than the first positive threshold, the loader is controlled to brake.

8. A loader path tracking control device, characterized in that: include: The acquisition module obtains the current status information and location information of the loader; The preview conversion module uses the preview algorithm to predict the position information of the loader at the next moment according to the current state information and position information, and converts the position information at the next moment into the position information in the Frenet coordinate system; The error calculation module calculates the lateral error and longitudinal error of the loader's position at the next moment compared with the planned path according to the position information in the Frenet coordinate system and the preset planned path; wherein the lateral error and longitudinal error at the next moment are respectively used as the lateral error and longitudinal error at the current moment in the loader path tracking control method at the next moment; The lateral control module uses an LQR controller to perform lateral control of the loader based on the lateral error at the current moment, the state information at the current moment, and the planned path if there is a lateral error at the next moment. The longitudinal control module uses a segmented PID algorithm to perform longitudinal control of the loader according to the range of the longitudinal error at the current moment, the current state information and the planned path if there is a longitudinal error at the next moment and the longitudinal error at the current moment is less than 0.

9. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores one or more programs, and the one or more programs include instructions. When the instructions are executed by a computing device, the computing device executes any one of the methods of claims 1 to 7.

10. A computer device, characterized in that: include: One or more processors and one or more memories, one or more programs are stored in the one or more memories and are configured to be executed by the one or more processors, and the one or more programs include instructions for executing any of the methods described in claims 1 to 7.

Images