Tractor path teaching and tractor navigation method based on path teaching

Abstract

The application provides a tractor path teaching method, records driving path data of a manually driven tractor passing through a target section, and analyzes and processes the driving path data to obtain an automatic driving route corresponding to the section; and also provides a tractor navigation method based on path teaching, which controls the automatic driving of the tractor based on the path reference points of the target path of manual teaching. The above scheme can ensure that the towing AVG does not interfere with the surrounding environment when carrying the material car, and realizes full-automatic towing material handling.

Description

Technical Field

[0001] This invention relates to the field of automatic control technology for tractor vehicles, and in particular to a tractor vehicle path teaching method and a tractor vehicle navigation method based on path teaching. Background Technology

[0002] Typically, towed AGVs are mainly used to attach material carts to the towed AGV, which then pulls the carts back and forth for transport. The towed AGV does not bear, or does not fully bear, the weight of the transported object. The advantage of this is that the material cart frame can be replaced like a train carriage; no modifications to the material cart are required, only the towed AGV itself. This is suitable for situations where materials are transported using trailers and trolleys, such as airport transport, chemical transport, and waste collection. However, because different material carts are mounted, the total length of the towed AGV and the material cart is uncertain. The navigation of the towed AGV only plans the route for the towed vehicle. When carrying a material cart, since the towed AGV's path planning does not consider the cart's length, the cart may encounter obstacles in its surroundings when traveling based on its planned route. Therefore, in existing technologies, when towed AGVs are integrated into fully automated systems to tow material carts for material transport, it is difficult to automatically generate the towed AGV's route and ensure that the route does not interfere with the surrounding environment, thus preventing fully automated towed material transport from being achieved. Summary of the Invention

[0003] To address the aforementioned problems, the present invention aims to provide a path teaching method for a tractor unit, capable of recording path information of the current road segment taught by a manual tractor unit, and generating navigation information for the corresponding road segment based on this path information. The present invention also aims to provide a tractor unit navigation method based on path teaching, which determines the navigation path of the tractor unit from its starting position to its target position based on the aforementioned path teaching method, and automatically controls the tractor unit to travel to the target position based on the navigation path. Since the situation of the material transport vehicle being carried is considered during tractor unit teaching, compared to the existing AVG-based automatic navigation method, it ensures that the tractor-mounted AVG will not interfere with the surrounding environment when carrying material transport vehicles, thus achieving fully automatic tractor-mounted material handling.

[0004] To achieve the above objectives, the present invention adopts the following technical solution:

[0005] A method for teaching a tractor path, the method comprising the following steps:

[0006] Record the driving path data of the manually driven tractor through the target road section at a preset frequency. The driving path data includes positioning data and motor operation data.

[0007] The driving path data is filtered to remove noise points;

[0008] The filtered driving path data is compressed and discretized to obtain key points;

[0009] The key points are fitted to obtain a spline curve;

[0010] The spline curve is discretized at a preset frequency to obtain a set of reference points for the target path, which are then stored in the teaching path database.

[0011] The above method allows for the acquisition of the driving path data of a manually driven tractor-trailer through a target road segment. This data is then processed to obtain a fitted spline curve, which serves as the automatic driving path for the corresponding target road segment. This automatic driving path can be used to control the tractor-trailer's passage through the corresponding road segment. Since the above tractor-trailer path teaching method is based on the automatic driving path determined by the manually driven tractor-trailer, the corresponding data takes into account the situation where the tractor-trailer is carrying a material vehicle. Driving according to this automatic driving path avoids interference between the towed AVG and the surrounding environment when carrying a material vehicle.

[0012] Preferably, the filtering of the driving path data to remove noise points includes:

[0013] For each type of driving path data, a corresponding error threshold is set, and for each type of driving path data, data with an error value greater than the corresponding category's error threshold is removed.

[0014] Filtering the recorded driving path data can reduce the amount of data processing while ensuring data accuracy and improving the efficiency of the method.

[0015] Preferably, the step of compressing and discretizing the filtered driving path data to obtain key points includes:

[0016] The positioning data in the driving path data includes multiple trajectory curve data. For each trajectory curve data, the Douglas-Peucker algorithm is used to traverse the data points on its trajectory curve, and the trajectory curve data is segmented according to a preset ratio. The maximum bow height value of each trajectory curve segment relative to the chord length of the trajectory curve is found as the key point to be retained.

[0017] There will be a lot of trajectory data in the filtered data above. The trajectory data is collected at a preset frequency. To ensure that the finally obtained automatic driving route is consistent with the manual driving route, the interval between two consecutive data collections is not very large. Therefore, the amount of trajectory data is very large. However, when analyzing the data, a too large amount of trajectory data will affect the operation efficiency, and many trajectory points are not necessary to be analyzed. Therefore, it is necessary to compress the data. Through the trajectory data compression process, the size of the trajectory data can be reduced without affecting the accuracy of the trajectory data.

[0018] Preferably, the specific processing method for each trajectory curve data includes:

[0019] (1) For each trajectory curve, connect a straight line AB between the two end points A and B of the trajectory curve. This straight line is the chord of the trajectory curve;

[0020] (2) Traverse all other points on the trajectory curve, calculate the distance from each point to the straight line AB, and find the point C with the maximum distance. The maximum distance is denoted as dmax;

[0021] (3) Compare the size of this distance dmax with the predefined threshold Dmax. If dmax < Dmax, then use the straight line AB as the approximation of the curve segment, and the curve segment processing is completed;

[0022] (4) If dmax >= Dmax, then make point C divide the trajectory curve AB into two segments AC and CB, and perform steps (1) to (3) on these two segments respectively;

[0023] (5) When all trajectory curves are processed, connect the broken lines formed by each segmentation point in sequence. The connected broken line is the path of the original curve, where the segmentation point is the key point.

[0024] The peak points in the trajectory curve are the key points in the trajectory curve. Usually, vehicle turning and other situations may occur at the peak points. Therefore, through the above steps of processing, the peak points in the trajectory curve can be screened out, ensuring that the driving path data retained after trajectory compression can accurately represent the driving information of the key points in the corresponding section.

[0025] Preferably, fitting the key points to obtain a spline curve includes: using the cubic spline fitting method to fit the key points, and using the spline curve obtained by fitting as the automatic driving route of the target section.

[0026] Using cubic spline fitting can avoid the Runge phenomenon when performing high-order processing on geometric points, and using the spline curve obtained by fitting the key points as the automatic driving route can control the tractor to drive automatically according to the automatic driving route.

[0027] Preferably, after receiving the start of manual teaching signal, the step of recording the driving path data of the manually driven tractor through the target road segment at a preset frequency is performed; after receiving the end of manual teaching signal, the recording of the driving data of the manually driven tractor through the target road segment is stopped.

[0028] The user can control whether to perform manual driving teaching by manually inputting a start or end signal for manual teaching.

[0029] A tractor navigation method based on path teaching, the method comprising:

[0030] Construct a teaching path database according to any of the above-described tractor path teaching methods;

[0031] The target path corresponding to the teaching path database is determined based on the starting and ending points of the tractor's transport, and the reference point set of the corresponding target path is obtained.

[0032] The tractor is navigated based on the set of reference points for the target path.

[0033] The aforementioned path-teaching-based tractor navigation method enables the tractor to automatically travel between the starting position and the target position along the route of a manually driven tractor. This avoids interference between the towed AVG and the surrounding environment when carrying material vehicles, thus achieving fully automated towing and material handling.

[0034] Preferably, navigating the tractor based on the reference point set of the target path includes:

[0035] Based on the kinematic model of the single steering wheel of the tractor, the standy algorithm is adopted. The set of reference points for the determined target path is used as input to plan and control the vehicle speed and angle in real time, so that the vehicle travels according to the target path.

[0036] Preferably, the standy algorithm is used, which takes the set of reference points for the determined target path as input and plans and controls the vehicle speed and angle in real time, including:

[0037] The current positioning point is taken as the initial pose state, and the point closest to the current positioning point in the reference point set of the target path is taken as the target pose state.

[0038] The standy algorithm is used as the control law to iterate until the error between the initial pose state and the target pose state converges to zero.

[0039] As a preferred embodiment, the kinematic model of the tractor is a single-steering wheel rear-wheel drive model, with the midpoint B of the line connecting the centers of the two rear wheels as the reference point. Its kinematic model can then be described as follows:

[0040]

[0041] Where (x, y) are the coordinates of the reference point of the tractor; β is the rudder angle of the tractor, and it is defined that counterclockwise is positive and clockwise is negative when viewed from the reference point towards the front wheels; L is the length of the tractor body; v A θ is the front wheel drive speed of the tractor; θ is the heading angle of the tractor, i.e., the angle between the vehicle's axis and the positive X-axis; ω is its heading angular velocity, defined as counterclockwise; the current pose of the tractor in the global coordinate system is represented by [x, y, θ]. T To express. Attached Figure Description

[0042] Figure 1 This is a flowchart of the tractor path teaching method in this embodiment;

[0043] Figure 2 This is a schematic diagram of the Douglas-Peucker algorithm in this embodiment;

[0044] Figure 3 This is a schematic diagram of cubic spline curve fitting in this embodiment;

[0045] Figure 4 This is a schematic diagram of the kinematic model control algorithm in this embodiment. Detailed Implementation

[0046] The embodiments of the present invention are described in detail below.

[0047] Example 1:

[0048] This embodiment provides a tractor path teaching method, which is used for tractor AVGs, such as... Figure 1 As shown, the method includes the following steps:

[0049] Record the driving path data of the manually driven tractor through the target road section at a preset frequency;

[0050] The host computer acquires the recorded driving route data;

[0051] The driving path data is filtered to remove noise points;

[0052] The filtered driving path data is compressed and discretized to obtain key points;

[0053] The key points are fitted to obtain a spline curve;

[0054] The spline curve is discretized at a preset frequency to obtain a set of reference points for the target path, which are then stored in the teaching path database.

[0055] The path teaching method in this embodiment is typically used for automated navigation on complex road sections. The tractor-driven AGV traverses the target road section manually in one go, recording manual driving data at a frequency of 10Hz during the process. In other embodiments, those skilled in the art can set other frequencies for recording manual driving data according to actual needs. In this embodiment, the AVG system records manual driving data, which includes positioning data and motor operating data. The driving path is determined based on the positioning data, and the motor operating status at the corresponding position is determined based on the motor operating data. In this embodiment, the manual driving data includes 3D laser positioning data, drive motor speed, steering motor angle, and road boundary information, which can be directly acquired by the tractor-driven AVG system.

[0056] In this embodiment, the driving path data is recorded on one side of the tractor and processed on the other side of the host computer. The host computer and / or the tractor are equipped with a start teaching button and a stop teaching button. Operating the start teaching button sends a start teaching signal, and operating the stop teaching button sends a stop teaching signal. In this embodiment, upon receiving the start manual teaching signal, the step of recording the driving path data of the manually driven tractor through the target road segment at a preset frequency is executed; upon receiving the stop manual teaching signal, the recording of the driving data of the manually driven tractor through the target road segment stops. In one embodiment, on the host computer's human-machine interface, a manual click to start teaching occurs. During the driving of the tractor-driven AGV along a complex path, the host computer records the 3D laser radar positioning data, drive motor speed, and steering motor angle in real time at a frequency of 10Hz, completing the manual driving of the teaching path in one go, and recording the above data in one go during this process. Upon completion of the driving, the teaching is completed by clicking "complete teaching".

[0057] The host computer obtains and processes the recorded driving path data. In this embodiment, it can send all driving path data of the current road segment at once after receiving the teaching end signal; or it can send the recorded driving path data in real time or at certain time intervals during the manual teaching driving process, and save the driving path data to the host computer after receiving the teaching end signal.

[0058] Since the manual driving data is recorded at a preset frequency, such as 10 Hz, in this embodiment, the amount of data is very large and there is a lot of noise. Therefore, it is necessary to filter the manual driving data. Specifically, error thresholds are set for the discrete data information of each category of the complete driving path data of the target road segment. The fluctuation values ​​of the positioning deviation from the actual position that are greater than the error threshold and the individual noise data of the motor deviating from the stable output speed are filtered out.

[0059] There will be a lot of trajectory data in the above filtered data. The trajectory data is collected at a frequency of 10HZ, so the amount of trajectory data is very large. However, when analyzing the data, a large amount of trajectory data will affect the operation efficiency, and many trajectory points are not necessary to analyze. Therefore, it is necessary to compress the data. The main goal of the trajectory data compression technology is to reduce the size of the trajectory data without affecting the accuracy of the trajectory data. In this embodiment, the Douglas-Peucker algorithm is used to compress and discretize the above filtered data to obtain key points. As Figure 2 shown, the positioning data in the driving path data includes multiple trajectory curve data. For each trajectory curve data, the Douglas-Peucker algorithm is used to traverse the data points on its trajectory curve, segment the trajectory curve data according to a preset ratio, and find the maximum arc height value relative to the chord length connected by the trajectory curve for each segment of the trajectory curve as the key points to be retained. The specific steps of the Douglas-Peucker algorithm are as follows:

[0060] (1) Connect a straight line AB between the two end points A and B of the curve on the trajectory curve. This straight line is the chord of the curve;

[0061] (2) Traverse all other points on the curve, find the distance from each point to the straight line AB, and find the point C with the maximum distance. The maximum distance is denoted as dmax;

[0062] (3) Compare the distance dmax with the predefined threshold Dmax. If dmax < Dmax, then use the straight line AB as the approximation of the curve segment, and the curve segment processing is completed;

[0063] (4) If dmax >= Dmax, then let point C divide the curve AB into two segments AC and CB, and perform steps (1) to (3) on these two segments respectively;

[0064] (5) When all curves are processed, connect the broken lines formed by each segmentation point in sequence, which is the path of the original curve. The above segmentation points are the peak points in the trajectory curve and are used as key points.

[0065] Fit the key points to obtain a spline curve, including: using the cubic spline fitting method to fit the key points, and using the spline curve obtained by fitting as the automatic driving route of the target section.

[0066] Usually, each geometric point is fitted and each geometric point will definitely pass through. However, the Runge phenomenon will occur in higher orders. Therefore, the present invention uses the cubic spline fitting method to fit the key points obtained by compressing and discretizing the above trajectory. The fitting solution steps are as follows:

[0067] When n+1 points are given, a single n-degree interpolation polynomial can be replaced by n cubic polynomials (usually lower), each polynomial defining a segment of the trajectory. The total function s(t) defined in this way is called a third-order spline curve. The functional form defining a cubic spline curve is:

[0068] s(t)={q k (t), t∈[t k , t k+1 ],k=0,...,n-1,

[0069] q k (t)=a k0 +a k1 (tt k )+a k2 (tt k ) 2 +a k3 (tt k ) 3

[0070] This trajectory consists of n cubic polynomials, and each polynomial requires the calculation of four parameters. Since n polynomials are necessary to define a trajectory passing through point n+1, the total number of coefficients to be determined is 4n. To solve this problem, the following conditions must be considered: 1. 2n conditions for given point interpolation, because each cubic function must pass through a point at its extremum; 2. n-1 conditions, the acceleration at the transition points must be continuous; 3. n-1 conditions, the acceleration at the transition points must be continuous; 4. In this way, 2n+2(n-1) conditions have been imposed, leaving 2 degrees of freedom unrestricted. Two more constraints are needed, which can be the initial and final velocities of the trajectory. The constraint equations under complete constraints are as follows:

[0071] q k (t k )=q k q k (t k+1 )=q k+1 k = 0, ..., n-1

[0072]

[0073]

[0074]

[0075] For each cubic spline curve segment, we have:

[0076]

[0077] By solving the equation above, we can obtain:

[0078]

[0079] A typical choice for calculating spline curves is to specify the initial and final velocities v0 and v... n Therefore, given point (t) k q k ), k = 0, ..., n; v k为 Velocity at a given point; coefficient a k，i Let i = 0, ..., 3; Tk = tk+1 - tk, the interval between two adjacent given points. This allows us to calculate the coefficients of each curve segment, thus obtaining the spline curve. The spline curve is then discretized at certain intervals to obtain a set of equally spaced spline curve trajectory points, which serves as the fully automated driving path information. This path is then added to the fully automated material handling route database.

[0080] Example 2:

[0081] This embodiment provides a tractor navigation method based on path teaching, the method comprising:

[0082] A teaching path database is constructed according to the tractor path teaching method described in Example 1;

[0083] The target path corresponding to the teaching path database is determined based on the starting and ending points of the tractor's transport, and the reference point set of the corresponding target path is obtained.

[0084] The tractor is navigated based on the set of reference points for the target path.

[0085] The aforementioned path-teaching-based tractor navigation method controls the tractor's automatic movement based on path reference points of the manually taught target path. This enables the tractor to automatically travel between the starting and target positions along the route manually driven, avoiding interference with the surrounding environment when the towed AVG is carrying material vehicles, thus achieving fully automated material handling.

[0086] In this embodiment, based on the starting point and destination of the traction AGV selected by the host computer, the host computer selects the corresponding path from the teaching path database and issues it to the traction AGV. Based on the kinematic model of the traction vehicle's single steering wheel, the standy algorithm is used, taking the fitted autonomous driving route database as input, to plan and control the vehicle's issuing speed and angle in real time, ensuring the vehicle strictly follows the issued route. The specific control method is as follows:

[0087] Based on the kinematic model of the tractor being a single-steering-wheel rear-drive model, and taking the midpoint B of the line connecting the centers of the two rear wheels as the reference point, its kinematic model can be described as follows:

[0088]

[0089] (x, y) are the coordinates of the reference point of the tractor. β is the rudder angle of the tractor, defined as positive counterclockwise and negative clockwise when viewed from the reference point towards the front wheels. L is the length of the tractor body. v A Let θ be the front wheel drive speed of the tractor, and θ be the yaw angle of the tractor, which is the angle between the vehicle's axis and the positive X-axis. ω is its angular velocity, defined as counterclockwise. The current pose of the tractor in the global coordinate system can be represented as [x, y, θ]. T To express.

[0090] During the movement of the tractor, such as Figure 4 As shown, the initial pose state can be [x, y, θ]. T This indicates that the target pose state can be represented as [x1, y1, θ1]. T This indicates that the pose error is [xe, ye, θe]. T Combining the kinematic model of the single-steering wheel tractor, the differential equation for its pose error during path tracking can be expressed as:

[0091]

[0092] From the above analysis, it can be concluded that the essence of the tractor's automatic following of the manually taught path is to determine the control law (v, ω). T Given an initial pose state (the current localization value) and a target pose state (the point in the set of reference points in the teaching path that is closest to the current localization value), the system, under the action of this control law, ensures that the pose error between the two is bounded and converges to 0, i.e., [xe, ye, θe]. T →[0, 0, 0] T The present invention uses the standy algorithm as the control law. After the vehicle obtains the data from the spline curve reference point set database, the control law iteration error converges to zero according to the standy algorithm. Based on the real-time calculation results, control commands, namely speed and angle commands, are issued to the vehicle motor at a preset frequency (25HZ in this embodiment) to control the vehicle to run along the manually taught path and realize fully automatic handling.

[0093] The following describes the solution of this embodiment in conjunction with specific application scenarios.

[0094] This section uses the application of a tractor-driven AGV in a railway maintenance station as an example to illustrate the vehicle's working process. Railway maintenance stations are located near railway lines in complex environments. Materials need to be transported from the maintenance warehouse to the unloading area next to the railway, and simultaneously retrieved from the picking area next to the railway back to the warehouse. Due to the long routes, complex and variable outdoor environment, and varying road conditions and widths, the tractor-driven trailer has a heavy load capacity of 6 tons. Therefore, in this environment, fully automated route creation involves complex, numerous, and variable input variables, which can easily lead to the trailer exceeding the road boundary when the tractor-driven vehicle travels along the route. Therefore, manual driving of the tractor-driven vehicle is employed on complex routes. The tractor-driven vehicle is manually driven from the start to the end of the complex route in one continuous operation, ensuring that it strictly follows the road centerline throughout the journey. During manual driving, the controller records positioning, motor speed, angle, and other data at a frequency of 10Hz. After the human completes the teaching route, the controller filters, compresses, discretizes, and fits the recorded data to obtain the autonomous driving route. Then, based on the vehicle kinematics model, the standby control algorithm is used to control the vehicle to drive along the autonomous driving route and complete the fully automated point-to-point transportation.

[0095] Furthermore, the traction AGV completes the automatic driving of the manually taught route through the above steps. Each complex route only needs to be taught once, and the controller can achieve permanent recording, which can achieve fully automatic material handling in any scenario.

[0096] In the description of this specification, references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of the invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples.

[0097] Although embodiments of the present invention have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those skilled in the art can make changes, modifications, substitutions and variations to the above embodiments within the scope of the present invention without departing from the principles and spirit of the present invention.

Claims

1. A method for teaching a tractor path, characterized in that, The method includes the following steps: Record the driving path data of the manned towing vehicle passing through the target section at a preset frequency, where the driving path data includes positioning data and motor operation data; Filter the driving path data to eliminate noise points; Compress and discretize the filtered driving path data to obtain key points; Fit the key points to obtain a spline curve; Discretize the spline curve at a preset frequency to obtain a reference point set of the target path and store it in the teaching path database; Among them, the step of compressing and discretizing the filtered driving path data to obtain key points includes: The positioning data in the driving path data includes multiple trajectory curve data. For each trajectory curve data, use the Douglas-Peucker algorithm to traverse the data points on its trajectory curve to obtain the key points to be retained; The step of fitting the key points to obtain a spline curve includes: using the cubic spline fitting method to fit the key points, and taking the fitted spline curve as the automatic driving route of the target section; The specific steps of the Douglas-Peucker algorithm processing include: (1) For each trajectory curve, connect a straight line AB between the two end points A and B of the trajectory curve. This straight line is the chord of the trajectory curve; (2) Traverse all other points on the trajectory curve, calculate the distance from each point to the straight line AB, and find the point C with the maximum distance. The maximum distance is denoted as dmax; (3) Compare the distance dmax with the pre-defined threshold Dmax. If dmax < Dmax, then take the straight line AB as the approximation of the curve segment, and the curve segment processing is completed; (4) If dmax >= Dmax, then let point C divide the trajectory curve AB into two segments AC and CB, and perform steps (1) to (3) on these two segments respectively; (5) When all trajectory curves are processed, connect the broken lines formed by the segmentation points in sequence, which is the path of the original curve, where the segmentation points are the key points.

2. The tractor path teaching method according to claim 1, characterized in that, The step of filtering the driving path data to eliminate noise points includes: Set corresponding error value thresholds for each type of driving path data respectively. For each type of driving path data, eliminate the data greater than the corresponding error threshold.

3. The tractor path teaching method according to claim 1, characterized in that, After receiving the start manual teaching signal, execute the step of recording the driving path data of the manned towing vehicle passing through the target section at a preset frequency; after receiving the end manual teaching signal, stop recording the driving data of the manned towing vehicle passing through the target section.

4. A tractor navigation method based on path teaching, characterized in that, The method includes: Construct a teaching path database according to the towing vehicle path teaching method described in any one of claims 1-3; Determine the corresponding target path in the teaching path database according to the starting point and ending point of the towing vehicle handling, and obtain the reference point set of the corresponding target path; Navigate the towing vehicle according to the reference point set of the target path.

5. The tractor navigation method based on path teaching according to claim 4, characterized in that, The step of navigating the towing vehicle according to the reference point set of the target path includes: Based on the kinematic model of the single steering wheel of the tractor, the standy algorithm is adopted. The set of reference points for the determined target path is used as input to plan and control the vehicle speed and angle in real time, so that the vehicle travels according to the target path.

6. The tractor navigation method based on path teaching according to claim 5, characterized in that, The standy algorithm is used, taking the set of reference points for the determined target path as input, to plan and control the vehicle speed and angle in real time, including: The current positioning point is taken as the initial pose state, and the point closest to the current positioning point in the reference point set of the target path is taken as the target pose state. The standy algorithm is used as the control law to iterate until the error between the initial pose state and the target pose state converges to zero.

7. A tractor navigation method based on path teaching according to claim 4 or 5, characterized in that, The kinematic model of the tractor is a single-steering-wheel rear-wheel drive model, with the midpoint B of the line connecting the centers of the two rear wheels as the reference point. Its kinematic model can then be described as follows: Where (x, y) are the coordinates of the reference point of the tractor; β is the rudder angle of the tractor, and it is defined that counterclockwise is positive and clockwise is negative when viewed from the reference point towards the front wheels; L is the length of the tractor body; v A θ is the front wheel drive speed of the tractor; θ is the heading angle of the tractor, i.e., the angle between the vehicle's axis and the positive X-axis; ω is its heading angular velocity, defined as counterclockwise; the current pose of the tractor in the global coordinate system is represented by [x, y, θ]. T To express.

Images