Robot path tracking method, medium and equipment

Abstract

The invention discloses a robot path tracking method, a medium and equipment, and belongs to the technical field of robot motion control. The technical problem that a navigation controller cannot obtain correct steering guidance due to the fact that an existing robot path tracking method does not construct coupling correlation between the position and the course angle of a robot is solved. Initial transverse deviation is obtained based on the actual course angle of the robot and the current operation path, and positive and negative symbols are given to the initial transverse deviation through the robot operation path point sequence, the real-time position and the real-time course angle to serve as transverse deviation; based on the agricultural machine front view point, the actual course angle and the operation path point sequence, the course deviation enabling the controller to select the minimum rotation angle is obtained. And weighting the course deviation and the transverse deviation to obtain a composite error, and outputting a correct steering instruction by a navigation controller according to a correction turning angle required for eliminating the composite error to realize robot path tracking. The method is mainly used for robot path tracking.

Description

Technical Field

[0001] This invention relates to the field of robot motion control technology, specifically to a robot path tracking method, medium, and device. Background Technology

[0002] With the rapid development and widespread application of precision agriculture technology, real-time kinematic-global navigation satellite system (RTK-GNSS) technology, based on the BeiDou Navigation Satellite System (BDS), has become a core standard feature of modern agricultural machinery due to its high precision and reliability. In actual field operations, agricultural machinery often needs to follow pre-set paths such as zigzag or bow-shaped patterns to complete reciprocating tasks such as tilling, sowing, and fertilizing. This places stringent requirements on the automatic navigation and path tracking accuracy of agricultural robots.

[0003] Existing path tracking methods generally rely on the principle of geometric projection to calculate navigation deviations, which include lateral deviation and heading deviation. Lateral deviation is defined as the minimum Euclidean distance from the robot's real-time position to the preset path, and its sign is determined solely by the geometric lateral direction determined by the cross product of vectors. This calculation method only reflects the static geometric position of the robot's body and does not incorporate crucial heading and attitude information. Heading deviation, on the other hand, requires pre-setting the forward look-ahead distance and selecting the corresponding forward look-ahead point, then obtaining it by solving for the difference between the robot's current heading angle and the path tangent angle at that forward look-ahead point.

[0004] This path tracking method relies solely on the geometric position of the agricultural robot relative to preset path points to determine navigation deviation and deviation sign. It lacks analysis of the coupling between the robot's position and heading angle, ignores the relative situation between the robot's real-time heading and the path direction, and fails to establish a coupling relationship between the deviation sign and the heading angle adjustment direction. As a result, the navigation controller cannot obtain the correct steering guidance under complex operating paths such as zigzag and bow-shaped paths. Summary of the Invention

[0005] To overcome the technical problem that existing robot path tracking methods fail to establish a coupling relationship between robot position and heading angle, resulting in the navigation controller's inability to obtain correct steering guidance, this application provides a robot path tracking method. Furthermore, this application also provides a corresponding computer-readable storage medium and device.

[0006] The first aspect of this invention provides a robot path tracking method, comprising:

[0007] Step S1: Real-time acquisition of the robot's work path point sequence and current position. and heading angle ;

[0008] Step S2: Based on the robot's work path point sequence and current position The first path point is obtained. First successor point and directed area Then based on the first path point First successor point Obtain the unsigned initial lateral deviation ; and then use the first path point Point to the first successor point vector and The angle between the positive and negative axes and robot heading angle and directed area Determine the unsigned initial lateral deviation The sign of the positive or negative sign, and the initial lateral deviation after assigning the sign. This is the lateral deviation. ;

[0009] Step S3: Based on the robot's work path point sequence and current position and heading angle The second path point is obtained. Second successor point Based on the second path point Second successor point and heading angle The heading deviation was obtained. ;

[0010] Step S4: Based on the lateral deviation and heading deviation Obtain the synthesis error Based on the synthesis error Get the corner The motion controller will correct the rotation angle. The signal is converted into a motor control signal, which drives the robot to turn the motor and achieve path tracking.

[0011] Furthermore, select the robot's work path point sequence that is most relevant to the current position. The path point with the minimum Euclidean distance and its successor are taken as the first path point. First successor point .

[0012] Furthermore, the directed area It is obtained through the following method:

[0013] Based on the first path point Point to the first successor point Path tangent vector First path point Point to the robot's current position deviation vector Determine the directed area .

[0014] Furthermore, the unsigned initial lateral deviation ,in and As an intermediate variable, , .

[0015] Furthermore, the unsigned initial lateral deviation The rules for determining the sign of a sign include:

[0016] Rule 1: When the included angle With the robot's heading angle satisfy And directional area At that time, the initial lateral deviation Assign a negative sign;

[0017] Rule 2: When the included angle With the robot's heading angle satisfy And directional area At that time, the initial lateral deviation Assign a positive sign;

[0018] Rule 3: When the included angle With the robot's heading angle satisfy And directional area At that time, the initial lateral deviation Assign a negative sign;

[0019] Rule 4: When the included angle With the robot's heading angle satisfy And directional area At that time, the initial lateral deviation Assign a positive sign;

[0020] Rule 5: When the included angle With the robot's heading angle satisfy And directional area At that time, the initial lateral deviation Assign a negative sign;

[0021] Rule Six: When the included angle With the robot's heading angle satisfy And directional area At that time, the initial lateral deviation Assign a positive sign;

[0022] Rule 7: When the included angle With the robot's heading angle satisfy And directional area At that time, the initial lateral deviation Assign a positive sign;

[0023] Rule 8: When the angle With the robot's heading angle satisfy And directional area At that time, the initial lateral deviation The value is 0;

[0024] Rule Nine: When the angle With the robot's heading angle satisfy And directional area At that time, the initial lateral deviation Assign a negative sign;

[0025] Rule 10: When the included angle With the robot's heading angle satisfy , And directional area At that time, the initial lateral deviation Assign a positive sign;

[0026] Rule 11: When the included angle With the robot's heading angle satisfy , And directional area At that time, the initial lateral deviation Assign a negative sign;

[0027] Rule 12: When the included angle With the robot's heading angle satisfy , And directional area At that time, the initial lateral deviation Assign a positive sign;

[0028] Rule Thirteen: When the included angle With the robot's heading angle satisfy , And directional area At that time, the initial lateral deviation Assign a negative sign;

[0029] Rule Fourteen: When the included angle With the robot's heading angle satisfy , And directional area At that time, the initial lateral deviation Assign a positive sign;

[0030] Rule 15: When the included angle With the robot's heading angle satisfy , And directional area At that time, the initial lateral deviation To assign a negative sign.

[0031] Further, step S3 includes:

[0032] Step S31: Based on the robot's current position and heading angle Determine the foreground viewpoint ;

[0033] Step S32: Select the robot's work path point sequence that corresponds to the forward view point. The path point with the minimum Euclidean distance and its successor are used as the second path point. Second successor point ;

[0034] Step S33: Based on the second path point Pointing to the second successor point vector and The angle between the positive and negative axes and robot heading angle Obtain the heading deviation .

[0035] Furthermore, step S33 yields the heading deviation. The process includes:

[0036] Rule A: When the included angle Heading angle and hour:

[0037] heading deviation The calculation method is as follows: ;

[0038] Rule B: When the included angle Heading angle and hour:

[0039] heading deviation The calculation method is as follows: ;

[0040] Rule C: When the included angle and heading angle hour:

[0041] heading deviation The calculation method is as follows: ;

[0042] Rule D: When the included angle Heading angle and hour:

[0043] heading deviation The calculation method is as follows: ;

[0044] Rule E: When the included angle Heading angle and hour:

[0045] heading deviation The calculation method is as follows: ;

[0046] Rule F: When the included angle and heading angle hour:

[0047] heading deviation The calculation method is as follows: .

[0048] Furthermore, the error described in step S4 , , These represent lateral deviations. Weighting coefficients, heading deviation The weighting coefficients.

[0049] A second aspect of the present invention provides a computer storage medium storing instructions that, when executed on a computing device, cause the computing device to perform the operation steps of the four-wheeled agricultural robot path tracking method described in the first aspect or any implementation thereof.

[0050] A third aspect of the present invention also provides a computing device, which may include a processor and a memory:

[0051] The memory is used to store computer programs;

[0052] The processor is configured to execute the method described in the first aspect and any embodiment of the first aspect according to the computer program.

[0053] Based on the implementation methods provided in the above aspects, this application can be further combined to provide more implementation methods.

[0054] The beneficial effects of this invention are:

[0055] This invention obtains the initial lateral deviation based on the robot's actual heading angle and the current working path. It assigns a positive or negative sign to the initial lateral deviation using the robot's working path point sequence, real-time position, and real-time heading angle, using this as the lateral deviation. Based on the agricultural machinery's forward viewpoint, actual heading angle, and working path point sequence, it obtains the heading deviation that allows the controller to select the minimum rotation angle. The heading deviation and lateral deviation are weighted to obtain a composite error. The navigation controller outputs the correct steering command based on the correction angle required to eliminate this composite error, thus achieving robot path tracking.

[0056] The robot path tracking method proposed in this invention establishes a coupled relationship between the robot's real-time position, heading angle, and working path when determining lateral deviation. This allows the sign of the lateral deviation to directly represent the turning intention toward the target, eliminating control logic conflicts under complex working conditions and improving the correction response speed. When determining heading deviation, the principle of minimum turning angle is introduced, effectively avoiding reverse circling of agricultural machinery and control oscillation drift caused by angle periodicity, and significantly improving the stability of path tracking. Attached Figure Description

[0057] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.

[0058] Figure 1 This is a schematic diagram illustrating the execution flow of a path tracking method for a four-wheeled agricultural robot in one embodiment of the method of the present invention;

[0059] Figure 2 In one embodiment of the method of the present invention, rule one A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path;

[0060] Figure 3 In one embodiment of the method of the present invention, rule one A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path;

[0061] Figure 4 In one embodiment of the method of the present invention, rule two A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path;

[0062] Figure 5 In one embodiment of the method of the present invention, rule two A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path;

[0063] Figure 6 In one embodiment of the method of the present invention, rule three A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path;

[0064] Figure 7 In one embodiment of the method of the present invention, rule three A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path;

[0065] Figure 8 In one embodiment of the method of the present invention, rule four A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path;

[0066] Figure 9 In one embodiment of the method of the present invention, rule four A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path;

[0067] Figure 10 In one embodiment of the method of the present invention, rule five A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path;

[0068] Figure 11 In one embodiment of the method of the present invention, rule five A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path;

[0069] Figure 12 In one embodiment of the method of the present invention, rule six A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path;

[0070] Figure 13 In one embodiment of the method of the present invention, rule six A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path;

[0071] Figure 14 In one embodiment of the method of the present invention, rule seven A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path;

[0072] Figure 15In one embodiment of the method of the present invention, rule seven A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path;

[0073] Figure 16 In one embodiment of the method of the present invention, rule eight A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path;

[0074] Figure 17 In one embodiment of the method of the present invention, rule eight A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path;

[0075] Figure 18 In one embodiment of the method of the present invention, rule nine A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path;

[0076] Figure 19 In one embodiment of the method of the present invention, rule nine A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path;

[0077] Figure 20 In one embodiment of the method of the present invention, rule ten... A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path;

[0078] Figure 21 In one embodiment of the method of the present invention, rule ten... A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path;

[0079] Figure 22 In one embodiment of the method of the present invention, rule eleven A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path;

[0080] Figure 23 In one embodiment of the method of the present invention, rule eleven A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path;

[0081] Figure 24 In one embodiment of the method of the present invention, rule twelve A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path;

[0082] Figure 25 In one embodiment of the method of the present invention, rule twelve A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path;

[0083] Figure 26In one embodiment of the method of the present invention, rule thirteen A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path;

[0084] Figure 27 In one embodiment of the method of the present invention, rule thirteen A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path;

[0085] Figure 28 In one embodiment of the method of the present invention, rule fourteen A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path;

[0086] Figure 29 In one embodiment of the method of the present invention, rule fourteen A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path;

[0087] Figure 30 In one embodiment of the method of the present invention, rule fifteen A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path;

[0088] Figure 31 In one embodiment of the method of the present invention, rule fifteen A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path;

[0089] Figure 32 This is a schematic diagram of a simulated actual furrow built in Simulink in one embodiment of the method of the present invention;

[0090] Figure 33 This is a simulation effect diagram of one embodiment of the method of the present invention;

[0091] Figure 34 This is a schematic diagram illustrating the absolute lateral deviation tracking of agricultural machinery when traveling on a straight road section, as shown in one embodiment of the method of the present invention.

[0092] Figure 35 This is a schematic diagram illustrating the absolute lateral deviation tracking of agricultural machinery when traveling on a curved road section, as shown in one embodiment of the method of the present invention. Detailed Implementation

[0093] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0094] The inventors of this invention discovered that existing path tracking methods generally rely on the principle of geometric projection to calculate navigation deviation. Lateral deviation is defined as the minimum Euclidean distance from the real-time position of the agricultural robot to all sequential points on a preset path, and its sign is determined solely by the geometric lateral direction determined by the cross product of vectors. This calculation method only reflects the static geometric position of the agricultural robot's body and does not incorporate crucial heading and attitude information. The calculation of heading deviation, however, requires pre-setting the forward-looking distance and selecting the corresponding forward-looking point, then obtaining the deviation by solving for the difference between the agricultural robot's current heading angle and the path tangential angle at that forward-looking point.

[0095] Meanwhile, the calculation of heading angle and path tangential angle neglects geometric angle characteristics and lacks an optimization mechanism for the shortest rotation path. Traditional algorithms often directly use the algebraic difference between the target heading and the current heading when calculating heading deviation. When this difference crosses the angle boundary, it produces a huge deviation, rather than the minimum angle deviation that fits the turning requirements of the agricultural robot. This method is prone to causing the controller to misjudge the turning direction, driving the agricultural robot to travel in the opposite direction along a long arc path, which in turn causes drastic changes in control commands and, in severe cases, may even lead to the agricultural robot going out of control.

[0096] To address the aforementioned issues, this embodiment provides a path tracking method for a four-wheeled agricultural robot. Please refer to [link / reference]. Figure 1 , Figure 1 This is a schematic diagram of the execution flow of a path tracking method for a four-wheeled agricultural robot according to the present invention, including the following steps:

[0097] Step S100: Real-time acquisition of the agricultural robot's operation path point sequence and current location. and heading angle .

[0098] In this step, the sequence of work path points and the current position The heading angle was obtained via RTK-GNSS. Acquired via an inertial measurement unit (IMU).

[0099] Step S200: Based on the sequence of work path points and current position of the agricultural robot. The first path point is obtained. First successor point and directed area Then based on the first path point First successor point Obtain the unsigned initial lateral deviation ; and then use the first path point Point to the first successor point vector and The angle between the positive and negative axes Agricultural robot heading angle and directed area Determine the unsigned initial lateral deviation The sign of the positive or negative sign, and the initial lateral deviation after assigning the sign. This is the lateral deviation. Specifically, it includes the following steps:

[0100] Step S201: Based on the sequence of work path points of the agricultural robot and its current position The first path point is obtained. First successor point .

[0101] Specifically, the current position of the agricultural robot is calculated by comparing all path points in the sequence of work path points. The Euclidean distance is used to select the agricultural robot's work path point sequence relative to the current position. The path point with the smallest Euclidean distance and its successor are taken as the first path point. First successor point .

[0102] The method used in this step to determine the first path point can eliminate local extremum interference.

[0103] Step S202: Based on the first path point First successor point Current location of agricultural robots Obtain the directed area . Specifically:

[0104] First, calculate the first path point. Point to the first successor point Path tangent vector With the first path point Pointing to the current location of the agricultural robot deviation vector .

[0105] The path tangent vector The expression is:

[0106]

[0107] The deviation vector The expression is:

[0108]

[0109] Then based on the path tangent vector Sum of deviation vectors Based on the principle of vector cross product, the directed area of ​​the agricultural robot's current position relative to the work path can be obtained. The directed area The expression is:

[0110]

[0111]

[0112] when If the agricultural robot is in a certain position, it is determined that its current position is on the left side of the preset path; if the position is not specified, it is determined that the robot is in a certain position. If the current position of the agricultural robot is determined to be on the right side of the preset path, then the robot is considered to be in a certain position.

[0113] Step S202: Based on the first path point First successor point Obtain the unsigned initial lateral deviation .

[0114] In this step, the unsigned initial lateral deviation The calculation formula includes:

[0115]

[0116]

[0117]

[0118] In the formula, and It is an intermediate variable.

[0119] Step S203: Utilize the first path point Point to the first successor point vector and The angle between the positive and negative axes Agricultural robot heading angle and directed area Determine the unsigned initial lateral deviation The sign of the positive or negative sign, and the initial lateral deviation after assigning the sign. This is the lateral deviation. .

[0120] In this step, the unsigned initial lateral deviation The rules for determining the sign of a sign include:

[0121] Rule 1: When the included angle With the heading angle of agricultural robots satisfy And directional area At that time, the initial lateral deviation Assign a negative sign;

[0122] Please refer to Figure 2 , Figure 3 , Figure 2 This is Rule 1 in this embodiment. A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path; Figure 3 This is Rule 1 in this embodiment. A schematic diagram showing the relative position and angle relationship between the agricultural machinery and the work path.

[0123] Rule 2: When the included angle With the heading angle of agricultural robots satisfy And directional area At that time, the initial lateral deviation Assign a positive sign;

[0124] Please refer to Figure 4 , Figure 5 , Figure 4 This is Rule 2 in this embodiment. A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path; Figure 5 This is Rule 2 in this embodiment. A schematic diagram showing the relative position and angle relationship between the agricultural machinery and the work path.

[0125] Rule 3: When the included angle Heading angle of agricultural robots satisfy And directional area At that time, the initial lateral deviation Assign a negative sign;

[0126] Please refer to Figure 6 , Figure 7 , Figure 6 This is the rule in the implementation example three. A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path; Figure 7 This is Rule 3 in this embodiment. A schematic diagram showing the relative position and angle relationship between the agricultural machinery and the work path.

[0127] Rule 4: When the included angle Heading angle of agricultural robots satisfy And directional area At that time, the initial lateral deviation Assign a positive sign;

[0128] Please refer to Figure 8 , Figure 9 , Figure 8 This is Rule 4 in this embodiment. A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path; Figure 9 This is Rule 4 in this embodiment. A schematic diagram showing the relative position and angle relationship between the agricultural machinery and the work path.

[0129] Rule 5: When the included angle Heading angle of agricultural robots satisfy And directional area At that time, the initial lateral deviation Assign a negative sign;

[0130] Please refer to Figure 10 , Figure 11 , Figure 10 This is Rule 5 in this embodiment. A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path; Figure 11 This is Rule 5 in this embodiment. A schematic diagram showing the relative position and angle relationship between the agricultural machinery and the work path.

[0131] Rule Six: When the included angle Heading angle of agricultural robots satisfy And directional area At that time, the initial lateral deviation Assign a positive sign;

[0132] Please refer to Figure 12 , Figure 13 , Figure 12 This is an embodiment of the method of the present invention, in rule six. A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path; Figure 13 This is an embodiment of the method of the present invention, in rule six. A schematic diagram showing the relative position and angle relationship between the agricultural machinery and the work path.

[0133] Rule 7: When the included angle Heading angle of agricultural robots satisfy And directional area At that time, the initial lateral deviation Assign a positive sign;

[0134] Please refer to Figure 14 , Figure 15 , Figure 14 This is Rule 7 in this embodiment. A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path; Figure 15 This is Rule 7 in this embodiment. A schematic diagram showing the relative position and angle relationship between the agricultural machinery and the work path.

[0135] Rule 8: When the angle Heading angle of agricultural robots satisfy And directional area At that time, the initial lateral deviation The value is 0;

[0136] Please refer to Figure 16 , Figure 17 , Figure 16 This is rule eight in this embodiment. A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path; Figure 17 This is rule eight in this embodiment. A schematic diagram showing the relative position and angle relationship between the agricultural machinery and the work path.

[0137] Rule Nine: When the angle Heading angle of agricultural robots satisfy And directional area At that time, the initial lateral deviation Assign a negative sign;

[0138] Please refer to Figure 18 , Figure 19 , Figure 18 This is Rule Nine in this embodiment. A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path; Figure 19 This is Rule Nine in this embodiment. A schematic diagram showing the relative position and angle relationship between the agricultural machinery and the work path.

[0139] Rule 10: When the included angle Heading angle of agricultural robots satisfy , And directional area At that time, the initial lateral deviation Assign a positive sign;

[0140] Please refer to Figure 20 , Figure 21 , Figure 20 This is Rule Ten in this embodiment. A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path; Figure 21 This is Rule Ten in this embodiment. A schematic diagram showing the relative position and angle relationship between the agricultural machinery and the work path.

[0141] Rule 11: When the included angle Heading angle of agricultural robots satisfy , And directional area At that time, the initial lateral deviation Assign a negative sign;

[0142] Please refer to Figure 22 , Figure 23 , Figure 22 This is Rule Eleven in this embodiment. A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path; Figure 23 It is in Rule Eleven of Implementation Examples A schematic diagram showing the relative position and angle relationship between the agricultural machinery and the work path.

[0143] Rule 12: When the included angle Heading angle of agricultural robots satisfy , And directional area At that time, the initial lateral deviation Assign a positive sign;

[0144] Please refer to Figure 24 , Figure 25 , Figure 24 This is Rule Twelve of this embodiment. A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path; Figure 25 This is Rule Twelve of this embodiment. A schematic diagram showing the relative position and angle relationship between the agricultural machinery and the work path.

[0145] Rule Thirteen: When the included angle Heading angle of agricultural robots satisfy , And directional area At that time, the initial lateral deviation Assign a negative sign;

[0146] Please refer to Figure 26 , Figure 27 , Figure 26 This is Rule Thirteen of this embodiment. A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path; Figure 27 This is Rule Thirteen of this embodiment. A schematic diagram showing the relative position and angle relationship between the agricultural machinery and the work path.

[0147] Rule Fourteen: When the included angle Heading angle of agricultural robots satisfy , And directional area At that time, the initial lateral deviation Assign a positive sign;

[0148] Please refer to Figure 28 , Figure 29 , Figure 28 This is the fourteenth rule of the embodiment. A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path; Figure 29 This is Rule Fourteen of this embodiment. A schematic diagram showing the relative position and angle relationship between the agricultural machinery and the work path.

[0149] Rule 15: When the included angle Heading angle of agricultural robots satisfy , And directional area At that time, the initial lateral deviation To assign a negative sign.

[0150] Please refer to Figure 30 , Figure 31 , Figure 30 This is Rule Fifteen of this embodiment. A schematic diagram showing the relative angular relationship between the agricultural machinery and the work path; Figure 31 This is Rule Fifteen of this embodiment. A schematic diagram showing the relative position and angle relationship between the agricultural machinery and the work path.

[0151] Initial lateral deviation after assigning a sign This is the lateral deviation. When lateral deviation When assigning a negative sign, the heading angle of the agricultural robot needs to be... Reduce; when the lateral deviation When assigning a positive sign, the heading angle of the agricultural robot needs to be set. Increase; when the lateral deviation When the value is 0, the heading angle of the agricultural robot needs to be maintained. constant.

[0152] In this step, the vector cross product method is used to calculate the vertical distance of the current position of the agricultural machinery relative to the path tangent as the unsigned initial lateral deviation. ; in determining the initial lateral deviation When determining the sign of the variable, the traditional rule of simply judging based on its geometric position on the left or right side of a preset path is abandoned. Instead, a lateral deviation is established. Value sign and heading angle of agricultural robot Adjusting the direct connection of the direction, thus reducing lateral deviation. The symbol directly represents the turning intention toward the target, eliminating control logic conflicts under complex operating conditions and improving the speed of correction response.

[0153] It should be noted that, based on the aforementioned unsigned lateral deviation With directed area The absolute lateral deviation can be obtained. ,in The sign function is represented by the absolute lateral deviation. This describes the distance between the agricultural machinery and the preset working path. A positive sign indicates the machinery is on the right side of the preset working path, and a negative sign indicates it is on the opposite side. This is due to the absolute lateral deviation. With directed area Since the signs of the positive and negative values ​​are the same, the rules for determining the sign of the unsigned initial lateral deviation (rules one through fifteen) can be used to determine the lateral deviation. The positive and negative judgments replace the directed area The determination of the positive and negative signs is the same, and will not be described in detail here.

[0154] Step S300: Based on the sequence of work path points and current position of the agricultural robot. and heading angle The second path point is obtained. Second successor point Based on the second path point Second successor point and heading angle The heading deviation was obtained. .

[0155] Specifically, this includes steps S301 to S303:

[0156] Step S301: Based on the current position of the agricultural robot and heading angle Determine the foreground viewpoint The expression is:

[0157]

[0158]

[0159] Step S302: Select the points from the agricultural robot's work path sequence that are closest to the forward viewpoint. The path point with the smallest Euclidean distance and its successor are taken as the second path point. Second successor point ;

[0160] Step S33: Based on the second path point Pointing to the second successor point vector and The angle between the positive and negative axes and the heading angle of agricultural robots Obtain the heading deviation .

[0161] The included angle The expression is:

[0162]

[0163] Due to the existence of angle Periodicity, at the included angle and heading angle The two span At boundaries, numerical abrupt changes are highly likely to occur. For example, when the included angle... Current heading angle The difference will be obtained by directly taking the subtraction. The calculated result, rather than the minimum steering difference that conforms to the physical meaning. This erroneous calculation result will cause the agricultural machinery to perform unexpected large-angle steering movements, resulting in errors of self-rotation and drift, which will ultimately damage the mechanical structure of the agricultural machinery.

[0164] This invention provides a shortest path normalization algorithm based on quadrant classification, which normalizes heading deviations. Forced constraints Within the interval, the agricultural machinery is always based on the stated heading deviation. Perform a steering maneuver to avoid unnecessary large-angle turns. The aforementioned heading deviation... The value of is determined by the included angle. and heading angle The rules for operation are jointly determined and include:

[0165] Rule A: When the included angle Heading angle and When, explain The value is not in Within this range, the minimum heading deviation required for the agricultural machinery to turn is not met; therefore, the following applies: Perform supplementary angle calculations to obtain the heading deviation. heading deviation This refers to the minimum deviation when the agricultural machinery performs a steering action.

[0166] The heading deviation described in Rule A The calculation expression is:

[0167]

[0168] Rule B: When the included angle Heading angle and When, explain The value is in Within the range, i.e., heading deviation heading deviation This refers to the minimum deviation when the agricultural machinery performs a steering action.

[0169] Rule C: When the included angle and heading angle When, explain The value is in Within the range, i.e., heading deviation heading deviation This refers to the minimum deviation when the agricultural machinery performs a steering action.

[0170] Rule D: When the included angle Heading angle and ,illustrate The value is not in Within this range, the minimum heading deviation required for the agricultural machinery to turn is not met; therefore, the following applies: Perform supplementary angle calculations to obtain the heading deviation. heading deviation This refers to the minimum deviation when the agricultural machinery performs a steering action.

[0171] The heading deviation described in Rule D The calculation expression is:

[0172]

[0173] Rule E: When the included angle Heading angle and hour, and The values ​​are all in Within the range, i.e., heading deviation heading deviation This refers to the minimum deviation when the agricultural machinery performs a steering action.

[0174] Rule F: When the included angle and heading angle hour, and The values ​​are all in Within the range, i.e., heading deviation heading deviation This refers to the minimum deviation when the agricultural machinery performs a steering action.

[0175] In this step, based on the vehicle length, the forward sight point is obtained by extending along the current heading angle of the agricultural machinery. The heading angle formed by the path point with the smallest Euclidean distance relative to the forward sight point and its successor point in the working path is searched as the expected heading of the agricultural machinery. The heading deviation between the expected heading and the actual heading of the agricultural machinery is then calculated. At the same time, the minimum angle principle is introduced for periodic normalization, which effectively avoids the agricultural machinery from going in the opposite direction due to the periodicity of the angle and controls the oscillation drift, thus significantly improving the stability of path tracking.

[0176] Step S400: Based on the lateral deviation and heading deviation Obtain the synthesis error Based on the synthesis error Get the corner The motion controller will correct the rotation angle. The signal is converted into a motor control signal to drive the steering motor of the front wheel of the agricultural machinery, thereby achieving path tracking.

[0177] Specifically, firstly, the controller measures the lateral deviation. and heading deviation Perform linear weighted summation to obtain the combined error. The synthesis error Input a PID control model, and the controller calculates the correction angle required to eliminate the error based on the preset PID parameters. The motion controller will correct the rotation angle. The signal is converted into a motor control signal to drive the steering motor of the front wheel of the agricultural machinery, thereby achieving path tracking.

[0178] The synthesis error The expression is:

[0179]

[0180] In the formula, , These represent lateral deviations. Heading deviation The weighting coefficients, in this embodiment, , Set to 1:1.

[0181] In this way, the change in the front wheel steering angle causes the agricultural machinery to generate a motion component that tends towards the target path, continuously reducing lateral and directional deviations in real time until the combined error is reduced. Approaching zero, path tracking is complete.

[0182] Furthermore, this application embodiment also provides a computer storage medium storing at least one instruction, which is loaded and executed by a processor to implement the aforementioned four-wheeled agricultural robot path tracking method.

[0183] It should be understood that the instructions include computer program products, software, or computerized methods corresponding to any method described in this invention; the instructions can be used to program computer systems or other electronic devices. Computer storage media may include readable media on which instructions are stored, and may include, but are not limited to, magnetic storage media, optical storage media; magneto-optical storage media include read-only memory (ROM), random access memory (RAM), erasable programmable memory (e.g., EPROM and EEPROM), and flash memory layers, or other types of media suitable for storing electronic instructions.

[0184] In addition, this application embodiment also provides a path tracking method generation device for a four-wheeled agricultural robot. The device includes a processor and a memory. It should be understood that it includes any device including a processor and a memory described in this invention. The device may also include other units and modules that perform display, interaction, processing, control and other functions through signals or instructions.

[0185] The memory stores at least one instruction, which is loaded and executed by the processor to implement the path tracking method for a four-wheeled agricultural robot.

[0186] Those skilled in the art will understand that at least one stored instruction constitutes a computer program product corresponding to a method or system. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code. The solutions in the embodiments of this application can be implemented using various computer languages, such as the object-oriented programming language Java and the interpreted scripting language JavaScript.

[0187] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means that implement the function of the method of the present invention.

[0188] These computer program instructions may 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 methods of the present invention.

[0189] To verify the beneficial effects of the present invention, the following simulation experiments were conducted:

[0190] In this embodiment, a kinematic model and path tracking control system for agricultural machinery were built on the MATLAB / Simulink simulation platform, and complex field operation conditions were simulated for testing.

[0191] This experiment constructed a closed-loop, straight-curve continuous operation path. Please refer to [reference needed]. Figure 32 , Figure 32 This is a schematic diagram of a simulated actual furrow built in Simulink for this experiment. The work path simulates a standard zigzag reciprocating operation between furrows: it consists of multiple parallel straight work segments and a semi-circular turning segment with a radius of 5m connected end-to-end. This work path covers agricultural machinery operation conditions including straight-line tracking, large-curvature entry into curves, heading correction upon exiting curves, and row switching, and can fully test the algorithm's adaptive capability under different curvatures. Simulation was performed using the four-wheeled agricultural robot path tracking method provided in this invention, and the results were obtained... Figure 33 .

[0192] With absolute lateral deviation As a standard for verifying accuracy, a schematic diagram of the lateral deviation of the method of this invention in straight and curved work sections is obtained. Please refer to... Figure 34 and Figure 35 , Figure 34 This is a schematic diagram illustrating the absolute lateral deviation tracking of agricultural machinery when it travels on a straight road in this experiment; Figure 35 This is a schematic diagram of the absolute lateral deviation tracking when the agricultural machinery is traveling on a curved road in this experiment.

[0193] from Figure 34 , 35 It can be seen that the method of the present invention maintains centimeter-level lateral deviation in both straight and curved work sections, overcoming the technical problem that existing agricultural robot path tracking methods do not establish a coupling relationship between the position and heading angle of the agricultural robot, resulting in the navigation controller being unable to obtain correct steering guidance under complex work paths such as zigzag and bow-shaped paths.

[0194] Finally, it should be noted that in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0195] As can be seen from the above description of the embodiments, those skilled in the art can clearly understand that all or part of the steps in the methods of the above embodiments can be implemented by means of software plus a general-purpose hardware platform. Based on this understanding, the technical solution of this application can be embodied in the form of a software product. This computer software product can be stored in a storage medium, such as a read-only memory (ROM) / RAM, magnetic disk, optical disk, etc., including several instructions to cause a computer device (which may be a personal computer, a server, or a network communication device such as a router) to execute the methods described in various embodiments or some parts of the embodiments of this application.

[0196] The above description of the disclosed embodiments enables those skilled in the art to make or use the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A robot path tracking method, characterized in that, include: Step S1: Real-time acquisition of the robot's work path point sequence and current position. and heading angle ; Step S2: Based on the robot's work path point sequence and current position The first path point is obtained. First successor point and directed area Then based on the first path point First successor point Obtain the unsigned initial lateral deviation ; and then use the first path point Point to the first successor point vector and The angle between the positive and negative axes and robot heading angle and directed area Determine the unsigned initial lateral deviation The sign of the positive or negative sign, and the initial lateral deviation after assigning the sign. This is the lateral deviation. ; Step S3: Based on the robot's work path point sequence and current position and heading angle The second path point is obtained. Second successor point Based on the second path point Second successor point and heading angle The heading deviation was obtained. ; Step S4: Based on the lateral deviation and heading deviation Obtain the synthesis error Based on the synthesis error Get the corner The motion controller will correct the rotation angle. The signal is converted into a motor control signal, which drives the robot to turn the motor and achieve path tracking.

2. The robot path tracking method according to claim 1, characterized in that, Select the robot's work path point sequence that is closest to the current position. The path point with the minimum Euclidean distance and its successor are taken as the first path point. First successor point .

3. The robot path tracking method according to claim 2, characterized in that, The directed area It is obtained through the following method: Based on the first path point Point to the first successor point Path tangent vector First path point Point to the robot's current position deviation vector Determine the directed area .

4. The robot path tracking method according to claim 3, characterized in that, The unsigned initial lateral deviation ,in and As an intermediate variable, , .

5. A robot path tracking method according to claim 4, characterized in that, The unsigned initial lateral deviation The rules for determining the sign of a sign include: Rule 1: When the included angle With the robot's heading angle satisfy And directional area At that time, the initial lateral deviation Assign a negative sign; Rule 2: When the included angle With the robot's heading angle satisfy And directional area At that time, the initial lateral deviation Assign a positive sign; Rule 3: When the included angle With the robot's heading angle satisfy And directional area At that time, the initial lateral deviation Assign a negative sign; Rule 4: When the included angle With the robot's heading angle satisfy And directional area At that time, the initial lateral deviation Assign a positive sign; Rule 5: When the included angle With the robot's heading angle satisfy And directional area At that time, the initial lateral deviation Assign a negative sign; Rule Six: When the included angle With the robot's heading angle satisfy And directional area At that time, the initial lateral deviation Assign a positive sign; Rule 7: When the included angle With the robot's heading angle satisfy And directional area At that time, the initial lateral deviation Assign a positive sign; Rule 8: When the angle With the robot's heading angle satisfy And directional area At that time, the initial lateral deviation The value is 0; Rule Nine: When the angle With the robot's heading angle satisfy And directional area At that time, the initial lateral deviation Assign a negative sign; Rule 10: When the included angle With the robot's heading angle satisfy , And directional area At that time, the initial lateral deviation Assign a positive sign; Rule 11: When the included angle With the robot's heading angle satisfy , And directional area At that time, the initial lateral deviation Assign a negative sign; Rule 12: When the included angle With the robot's heading angle satisfy , And directional area At that time, the initial lateral deviation Assign a positive sign; Rule Thirteen: When the included angle With the robot's heading angle satisfy , And directional area At that time, the initial lateral deviation Assign a negative sign; Rule Fourteen: When the included angle With the robot's heading angle satisfy , And directional area At that time, the initial lateral deviation Assign a positive sign; Rule 15: When the included angle With the robot's heading angle satisfy , And directional area At that time, the initial lateral deviation To assign a negative sign.

6. The robot path tracking method according to claim 5, characterized in that, Step S3 includes: Step S31: Based on the robot's current position and heading angle Determine the foreground viewpoint ; Step S32: Select the robot's work path point sequence that corresponds to the forward view point. The path point with the minimum Euclidean distance and its successor are used as the second path point. Second successor point ; Step S33: Based on the second path point Pointing to the second successor point vector and The angle between the positive and negative axes and robot heading angle Obtain the heading deviation .

7. A robot path tracking method according to claim 6, characterized in that, Step S33 yields the heading deviation. The process includes: Rule A: When the included angle Heading angle and hour: heading deviation The calculation method is as follows: ; Rule B: When the included angle Heading angle and hour: heading deviation The calculation method is as follows: ; Rule C: When the included angle and heading angle hour: heading deviation The calculation method is as follows: ; Rule D: When the included angle Heading angle and hour: heading deviation The calculation method is as follows: ; Rule E: When the included angle Heading angle and hour: heading deviation The calculation method is as follows: ; Rule F: When the included angle and heading angle hour: heading deviation The calculation method is as follows: .

8. A robot path tracking method according to claim 7, characterized in that, The error described in step S4 , , These represent lateral deviations. Weighting coefficients, heading deviation The weighting coefficients.

9. A computer storage medium, characterized in that, The storage medium stores at least one instruction, which is loaded and executed by a processor to implement a robot path tracking method according to any one of claims 1 to 8.

10. A robot path tracking method generation device, characterized in that, The device includes a processor and a memory, the memory storing at least one instruction, which is loaded and executed by the processor to implement a robot path tracking method according to any one of claims 1 to 8.

Images