Agricultural robot driving path control method and system

By obtaining the actual center of mass position and heading angle of the agricultural robot and adjusting the angle with the fuzzy rules of the change theory domain, the problem of driving path deviation of the agricultural robot is solved, and the accuracy of path tracking and anti-interference ability are improved.

CN115328139BActive Publication Date: 2024-12-03SHIHEZI UNIVERSITY
View PDF 4 Cites 0 Cited by

Patent Information

Application Number
CN202211000865.9
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2022-08-19
Publication Date
2024-12-03
Estimated Expiration
2042-08-19

AI Technical Summary

Technical Problem

The driving path of agricultural robots has a large deviation from the preset path in complex plantation environments, resulting in inefficiency and high cost.

Method used

By obtaining the marking point position and actual center of mass position in the preset driving path of the target robot, combining the actual heading angle and front wheel angle, the angle correction parameters are calculated using the variable theory domain fuzzy rules, and the motor steering vector is adjusted to reduce the driving path deviation.

Benefits of technology

Real-time path adjustment of agricultural robots is realized, the deviation between the actual driving path and the preset path is reduced, and the accuracy of path tracking and anti-external interference capability are improved.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN115328139B_ABST
    Figure CN115328139B_ABST
Patent Text Reader

Abstract

The present application provides a method and system for controlling the driving path of an agricultural robot. The method comprises: obtaining the first mark point position and the second mark point position in the preset driving path of the target robot, respectively obtaining the actual center of mass position, the actual heading angle and the actual front wheel turning angle of the target robot at each of two adjacent moments; respectively determining the actual front wheel turning angle error at each of two adjacent moments and the actual front wheel turning angle error change between two adjacent moments according to the first mark point position, the second mark point position, the actual center of mass position at two adjacent moments, the actual heading angle at two adjacent moments, the actual front wheel turning angle at two adjacent moments and the preset formula; determining the turning angle correction parameter at the next moment according to the actual front wheel turning angle error at the current moment, the actual front wheel turning angle error change between two adjacent moments and the variable domain fuzzy rule, and determining the steering amount of the motor on the target robot at the next moment according to the actual front wheel turning angle error at two adjacent moments and the turning angle correction parameter.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present application relates to the field of robot control technology, and in particular to a method and system for controlling a driving path of an agricultural robot. Background Art

[0002] At present, the fertilization and harvesting of fruit trees are mostly done manually, but the labor cost is high and the efficiency is low. For example, when farmers plant fruit trees on a large scale and use manual fertilization, the cost of fertilization is high and the efficiency is low, which seriously restricts the development of the planting industry.

[0003] For this reason, agricultural machinery came into being, such as agricultural robots that can fertilize, agricultural robots that can harvest, etc. The application of agricultural machinery can accelerate the process of mechanization of planting industry and has a positive role in promoting modern agricultural production.

[0004] The driving path of the agricultural robot is pre-planned. However, due to the complex and changeable environment of the plantation, there is a large deviation between the actual driving path of the agricultural robot and the preset driving path. Summary of the invention

[0005] The present application provides a method and system for controlling the driving path of an agricultural robot, and the technical solution is as follows:

[0006] In the first aspect, the present application provides a method for controlling a driving path of an agricultural robot, the method comprising: obtaining a first marking point position and a second marking point position in a preset driving path of a target robot, and respectively obtaining the actual center of mass position, actual heading angle and actual front wheel turning angle of the target robot at each of two adjacent moments, the two adjacent moments being respectively: the current moment and the previous moment; determining the actual front wheel turning angle error at each of the two adjacent moments and the actual front wheel turning angle error change between the two adjacent moments according to the first marking point position, the second marking point position, the actual center of mass position at the two adjacent moments, the actual heading angle at the two adjacent moments, the actual front wheel turning angle at the two adjacent moments and a preset formula; determining a turning angle correction parameter at the next moment according to the actual front wheel turning angle error at the current moment, the actual front wheel turning angle change between the two adjacent moments and a variable domain fuzzy rule, and determining the steering amount of the motor on the target robot at the next moment according to the actual front wheel turning angle error at the current moment, the actual front wheel turning angle error at the previous moment and the turning angle correction parameter at the next moment, so as to adjust the heading of the target robot.

[0007] In some embodiments, the steering angle correction parameter at the next moment is determined based on the actual front wheel steering angle error at the current moment, the actual front wheel steering angle error change between two adjacent moments and the variable universe fuzzy rule, including: determining the telescopic factor based on the actual front wheel steering angle error at the current moment and the actual front wheel steering angle error change between two adjacent moments; determining the steering angle correction parameter at the next moment based on the actual front wheel steering angle error at the current moment, the actual front wheel steering angle error change between two adjacent moments, the telescopic factor and the variable universe fuzzy rule.

[0008] In some embodiments, the actual front wheel turning angle error at each of the two adjacent moments and the actual front wheel turning angle error change between the two adjacent moments are determined according to the first mark point position, the second mark point position, the actual center of mass position at the two adjacent moments, the actual heading angle at the two adjacent moments, the actual front wheel turning angle at the two adjacent moments and a preset formula, including: determining the expected front wheel turning angle at the previous moment according to the coordinates of the first mark point position, the coordinates of the second mark point position, the coordinates of the actual center of mass position at the previous moment, and the actual heading angle at the previous moment, and determining the actual front wheel turning angle error at the previous moment according to the expected front wheel turning angle at the previous moment and the actual front wheel turning angle at the previous moment; determining the expected front wheel turning angle at the current moment according to the coordinates of the first mark point position, the coordinates of the second mark point position, the coordinates of the actual center of mass position at the current moment, and the actual heading angle at the current moment, and determining the actual front wheel turning angle error at the current moment according to the expected front wheel turning angle at the current moment and the actual front wheel turning angle at the current moment; determining the actual front wheel turning angle error change between the two adjacent moments according to the actual front wheel turning angle error at the previous moment and the actual front wheel turning angle error at the current moment.

[0009] In some embodiments, the expected turning angle of the front wheel at a certain moment is determined according to the coordinates of the first marking point position, the coordinates of the second marking point position, the coordinates of the actual center of mass position at a certain moment, the actual heading angle at a certain moment, and the following formula:

[0010]

[0011]

[0012] d θ =(1-λ1)k1e d +λ1k2e θ

[0013] Among them, e d is the lateral deviation at a certain moment, (x1, y1) is the coordinate of the first marking point, (x2, y2) is the coordinate of the second marking point, (x3, y3) is the coordinate of the actual center of mass at a certain moment, e θ is the heading angle deviation at a certain moment, θ is the actual heading angle at a certain moment, d θis the expected turning angle of the front wheel at a certain moment, λ1 is the deviation weight, k1 is the lateral deviation gain at a certain moment, and k2 is the heading angle deviation gain at a certain moment.

[0014] In some embodiments, the actual front wheel steering angle error at the last moment is determined according to the expected front wheel steering angle at the last moment, the actual front wheel steering angle at the last moment, and the following formula:

[0015] e(t-1)=d θ (t-1)-m θ (t-1)

[0016] The actual front wheel steering angle error at the current moment is determined based on the expected front wheel steering angle at the current moment, the actual front wheel steering angle at the current moment, and the following formula:

[0017] e(t)=d θ (t)-m θ (t)

[0018] The actual front wheel steering angle error change between two adjacent moments is determined based on the actual front wheel steering angle error at the previous moment, the actual front wheel steering angle error at the current moment, and the following formula:

[0019] ec(t)=e(t)-e(t-1)

[0020] Among them, e(t-1) is the actual front wheel steering angle error at the previous moment, d θ (t-1) is the expected turning angle of the front wheel at the previous moment, m θ (t-1) is the actual front wheel steering angle at the previous moment, e(t) is the actual front wheel steering angle error at the current moment, d θ (t) is the expected turning angle of the front wheel at the current moment, m θ (t) is the actual front wheel steering angle at the current moment, and ec(t) is the actual front wheel steering angle error change between two adjacent moments.

[0021] In some embodiments, the angle correction parameter at the next moment is determined according to the actual front wheel angle error at the current moment, the change of the actual front wheel angle error between two adjacent moments and the variable domain fuzzy rule, and the steering amount of the motor on the target robot at the next moment is determined according to the actual front wheel angle error at the current moment, the actual front wheel angle error at the previous moment and the angle correction parameter at the next moment to adjust the heading of the target robot, including: respectively determining the basic domain of the actual front wheel angle error at the current moment, the change of the actual front wheel angle error between two adjacent moments, the first angle parameter, the second angle parameter and the third angle parameter; determining the steering amount of the motor on the target robot at the next moment according to the actual front wheel angle error at the current moment, the actual front wheel angle error at the previous moment and the angle correction parameter at the next moment to adjust the heading of the target robot, including: respectively determining the basic domain of the actual front wheel angle error at the current moment, the change of the actual front wheel angle error between two adjacent moments, the first angle parameter, the second angle parameter and the third angle parameter; determining the steering amount of the motor on the target robot at the next moment according to the actual front wheel angle error at the current moment, the actual front wheel angle error between two adjacent moments The first expansion factor and the second expansion factor are determined according to the basic domain of the actual front wheel angle error at the current moment, the basic domain of the actual front wheel angle error change between two adjacent moments, the basic domain of the first angle parameter, the basic domain of the second angle parameter, the basic domain of the third angle parameter, the first expansion factor, the second expansion factor and the variable domain fuzzy rule; the steering amount of the motor on the target robot at the next moment is determined according to the actual front wheel angle error at the current moment, the actual front wheel angle error change between two adjacent moments, the first angle correction parameter, the second angle correction parameter and the third angle correction parameter.

[0022] In some embodiments, the first telescoping factor and the second telescoping factor are determined according to the actual front wheel steering angle error at the current moment, the actual front wheel steering angle error change between two adjacent moments, and the following formula:

[0023]

[0024]

[0025] The steering amount of the motor on the target robot at the next moment is determined according to the actual front wheel angle error at the current moment, the change in the actual front wheel angle error between two adjacent moments, the first angle correction parameter, the second angle correction parameter, the third angle correction parameter and the following formula:

[0026]

[0027] Among them, α(e t ) is the first scaling factor, λ2 is the adjustment weight, 0<λ2<1, e is a natural constant, k3 is the adjustment coefficient, β(e ct ) is the second telescopic factor, λ3 is the adjustment weight, 0<λ3<1, k4 is the adjustment coefficient, u is the steering amount of the motor on the target robot at the next moment, Δk P , Δk I and ΔkD They are respectively the first corner correction parameter, the second corner correction parameter, and the third corner correction parameter.

[0028] In a second aspect, the present application provides an agricultural robot driving path control system, which adopts any of the above methods, and the system includes: a data acquisition module, a data fusion module and a data output module, the data acquisition module is used to obtain the first mark point position and the second mark point position in the preset driving path of the target robot, and respectively obtain the actual center of mass position, actual heading angle and actual front wheel turning angle of the target robot at two adjacent moments, and the two adjacent moments are: the current moment and the previous moment; the data fusion module is used to determine the actual front wheel turning angle error at each of the two adjacent moments and the actual front wheel turning angle error change between the two adjacent moments according to the first mark point position, the second mark point position, the actual center of mass position at the two adjacent moments, the actual heading angle at the two adjacent moments and the actual front wheel turning angle at the two adjacent moments; the data output module is used to determine the angle correction parameter at the next moment according to the actual front wheel turning angle error at the current moment, the actual front wheel turning angle error change between the two adjacent moments and the variable domain fuzzy rule, and determine the steering amount of the motor on the target robot at the next moment according to the angle correction parameter at the next moment to adjust the heading of the target robot.

[0029] In a third aspect, the present application provides an electronic device, comprising: a processor, a memory, and a bus; wherein the processor and the memory communicate with each other via the bus; and the processor is used to call program instructions in the memory to execute any of the methods described above.

[0030] In a fourth aspect, the present application provides a computer-readable storage medium, the computer-readable storage medium comprising: a stored program; wherein, when the program is running, the device where the storage medium is located is controlled to execute any of the methods described above.

[0031] Compared with the prior art, the agricultural robot driving path control method provided in the first aspect of the present application determines the steering amount of the motor on the target robot at the next moment by combining the actual front wheel angle error at two adjacent moments and the angle correction parameter at the next moment, thereby being able to adjust the steering amount of the motor on the target robot at the next moment in real time according to the real-time changing front wheel angle error, so as to reduce the deviation between the actual driving path and the preset driving path.

[0032] The system provided in the second aspect, the electronic device provided in the third aspect, and the computer-readable storage medium provided in the fourth aspect of the present application have the same or similar beneficial effects as the method provided in the first aspect. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] By reading the detailed description below with reference to the accompanying drawings, the above and other purposes, features and advantages of the exemplary embodiments of the present application will become easy to understand. In the accompanying drawings, several embodiments of the present application are shown in an exemplary and non-limiting manner, and the same or corresponding reference numerals represent the same or corresponding parts, wherein:

[0034] Figure 1 A flow chart of a method for controlling a driving path of an agricultural robot according to some embodiments of the present application;

[0035] Figure 2 A coordinate diagram of the first marking point position, the second marking point position and the actual center of mass position of the target robot at a certain moment in some embodiments of the present application;

[0036] Figure 3 A variable domain fuzzy rule table for some embodiments of the present application;

[0037] Figure 4 A system block diagram of variable domain fuzzy rules in some embodiments of the present application. DETAILED DESCRIPTION

[0038] The exemplary embodiments of the present application will be described in more detail below with reference to the accompanying drawings. Although the exemplary embodiments of the present application are shown in the accompanying drawings, it should be understood that the present application can be implemented in various forms and should not be limited by the embodiments described herein. On the contrary, these embodiments are provided in order to enable a more thorough understanding of the present application and to fully convey the scope of the present application to those skilled in the art.

[0039] It should be noted that, unless otherwise specified, the technical terms or scientific terms used in this application should have the common meanings understood by technicians in the field to which this application belongs.

[0040] First aspect

[0041] The present application embodiment provides a method for controlling the driving path of an agricultural robot. Figure 1 As shown, the method includes:

[0042] S101: Obtain the first mark point position and the second mark point position in the preset driving path of the target robot, and respectively obtain the actual center of mass position, actual heading angle and actual front wheel turning angle of the target robot at each of two adjacent moments, where the two adjacent moments are: the current moment and the previous moment;

[0043] S102: determining the actual front wheel steering angle error at each of two adjacent moments and the actual front wheel steering angle error change between two adjacent moments according to the first marking point position, the second marking point position, the actual center of mass position at two adjacent moments, the actual heading angle at two adjacent moments, the actual front wheel steering angle at two adjacent moments, and a preset formula;

[0044] S103: Determine the steering angle correction parameter at the next moment according to the actual front wheel steering angle error at the current moment, the change of the actual front wheel steering angle error between two adjacent moments and the variable domain fuzzy rule, and determine the steering amount of the motor on the target robot at the next moment according to the actual front wheel steering angle error at the current moment, the actual front wheel steering angle error at the previous moment and the steering angle correction parameter at the next moment to adjust the heading of the target robot.

[0045] Specifically, the agricultural robot mentioned above may be a robot that can spray medicine, a robot that can weed, a robot that can fertilize, a robot that can pick fruits, or a robot with other functions.

[0046] The preset driving path mentioned above can be a driving path planned by the upper controller, or a driving path planned manually. The first mark point and the second mark point mentioned above are two different points in the preset driving path of the target robot, that is, the first mark point position and the second mark point position are two different positions in the preset driving path of the target robot, for example: Figure 2 As shown, a part of the preset driving path is line segment AB, A is the starting point in the path segment, B is the ending point in the path segment, and the first marking point is point A, the second marking point is point B, the position of the first marking point can be represented by the coordinates of point A (x1, y1), and the position of the second marking point can be represented by the coordinates of point B (x1, y2). In the above, the actual center of mass position, actual heading angle and actual front wheel turning angle of the target robot at each of two adjacent moments are obtained respectively, and the two adjacent moments are: the current moment and the previous moment, that is, the actual center of mass position, actual heading angle and actual front wheel turning angle of the target robot at the current moment are obtained, and the actual center of mass position, actual heading angle and actual front wheel turning angle of the target robot at the previous moment are obtained. Here, the actual center of mass position is the position of the mass center of the target robot, for example: see Figure 2 As shown, the actual center of mass position of the target robot at the current moment is the position of point C, and the actual center of mass position can be represented by the coordinates of point C. It should be noted that the actual heading angle and the actual front wheel turning angle can be obtained by instruments such as angle sensors.

[0047] In the above, the actual front wheel turning angle error at each of the two adjacent moments and the actual front wheel turning angle error change between the two adjacent moments are determined respectively according to the first mark point position, the second mark point position, the actual center of mass position at each of the two adjacent moments, the actual heading angle at each of the two adjacent moments, the actual front wheel turning angle at each of the two adjacent moments and the preset formula. That is to say, the actual front wheel turning angle error at the current moment, the actual front wheel turning angle error at the previous moment and the actual front wheel turning angle error change between the current moment and the previous moment can be determined respectively according to the first mark point position, the second mark point position, the actual center of mass position at each of the two adjacent moments, the actual heading angle at each of the two adjacent moments, the actual front wheel turning angle at each of the two adjacent moments and the preset formula. Two preset formulas are proposed in the present application. Of course, other preset formulas can also be used in the specific implementation, which is not particularly limited here.

[0048] Preset formula 1, reference Figure 2 :

[0049]

[0050]

[0051]

[0052] d θ =(1-λ1)e d +λ1e θ

[0053] e(t-1)=d θ (t-1)-m θ (t-1)

[0054] e(t)=d θ (t)-m θ (t)

[0055] ec(t)=e(t)-e(t-1)

[0056] Among them, e d is the lateral deviation at a certain moment, (x1, y1) is the coordinate of the first marking point, (x2, y2) is the coordinate of the second marking point, (x3, y3) is the coordinate of the actual center of mass at a certain moment, e θ is the heading angle deviation at a certain moment, θ is the actual heading angle at a certain moment, d θ is the expected front wheel steering angle at a certain moment, λ1 is the deviation weight, e(t-1) is the actual front wheel steering angle error at the previous moment, d θ (t-1) is the expected turning angle of the front wheel at the previous moment, m θ(t-1) is the actual front wheel steering angle at the previous moment, e(t) is the actual front wheel steering angle error at the current moment, d θ (t) is the expected turning angle of the front wheel at the current moment, m θ (t) is the actual front wheel steering angle at the current moment, and ec(t) is the actual front wheel steering angle error change between two adjacent moments. It should be noted that λ1 can be the optimal value of multiple experiments recorded in the database, or it can be a value set based on experience.

[0057] Preset formula 2, reference Figure 2 :

[0058]

[0059]

[0060]

[0061] d θ =(1-λ1)k1e d +λ1k2e θ

[0062] e(t-1)=d θ (t-1)-m θ (t-1)

[0063] e(t)=d θ (t)-m θ (t)

[0064] ec(t)=e(t)-e(t-1)

[0065] Among them, e d is the lateral deviation at a certain moment, (x1, y1) is the coordinate of the first marking point, (x2, y2) is the coordinate of the second marking point, (x3, y3) is the coordinate of the actual center of mass at a certain moment, e θ is the heading angle deviation at a certain moment, θ is the actual heading angle at a certain moment, d θ is the expected front wheel steering angle at a certain moment, λ1 is the deviation weight, k1 is the lateral deviation gain at a certain moment, k2 is the heading angle deviation gain at a certain moment, e(t-1) is the actual front wheel steering angle error at the previous moment, d θ (t-1) is the expected turning angle of the front wheel at the previous moment, m θ (t-1) is the actual front wheel steering angle at the previous moment, e(t) is the actual front wheel steering angle error at the current moment, d θ (t) is the expected turning angle of the front wheel at the current moment, m θ(t) is the actual front wheel steering angle at the current moment, and ec(t) is the actual front wheel steering angle error change between two adjacent moments. It should be noted that λ1, k1, and k2 can be the optimal values ​​of multiple experiments recorded in the database, or can be values ​​set based on experience.

[0066] In the above, the angle correction parameter at the next moment is determined according to the actual front wheel angle error at the current moment, the actual front wheel angle error change between two adjacent moments and the variable domain fuzzy rule. Here, the variable domain fuzzy rule follows the rule of variable domain fuzzy PID idea, and the determined angle correction parameter can be combined with the actual front wheel angle error at the current moment and the actual front wheel angle error change between two adjacent moments to determine the steering amount of the motor on the target robot at the next moment, thereby adjusting the heading of the target robot to make the heading of the target robot adapt to the actual situation of the plantation road.

[0067] In this embodiment, firstly, the steering amount of the motor on the target robot at the next moment is determined by combining the actual front wheel angle error at two adjacent moments and the angle correction parameter at the next moment, so that the steering amount of the motor on the target robot at the next moment can be adjusted in real time according to the front wheel angle error in real time, so as to reduce the deviation between the actual driving path and the preset driving path. Secondly, on the basis of the conventional fuzzy PID controller, the idea of ​​variable domain fuzzy PID is introduced, which greatly improves the performance of resisting external interference, solves the problem that the PID algorithm of the conventional path tracking model will introduce external interference and cause the control system to be unstable when there is external interference, so as to more accurately control the movement of the agricultural robot.

[0068] In some embodiments, the steering angle correction parameter at the next moment is determined according to the actual front wheel steering angle error at the current moment, the change in the actual front wheel steering angle error between two adjacent moments, and the variable domain fuzzy rule, including:

[0069] Determine the expansion and contraction factor according to the actual front wheel steering angle error at the current moment and the change in the actual front wheel steering angle error between two adjacent moments;

[0070] The steering angle correction parameter at the next moment is determined according to the actual front wheel steering angle error at the current moment, the actual front wheel steering angle error change between two adjacent moments, the telescopic factor and the variable domain fuzzy rule.

[0071] Specifically, the expansion factor is determined according to the actual front wheel steering angle error at the current moment and the actual front wheel steering angle error change between two adjacent moments. It can be determined by comparing the actual front wheel steering angle error at the current moment and the actual front wheel steering angle error change between two adjacent moments with previous experimental data, so as to find a number close to the actual front wheel steering angle error at the current moment and the actual front wheel steering angle error change between two adjacent moments in the previous data, and the expansion factor in the closest experiment is used as the expansion factor of this embodiment.

[0072] The expansion factor is determined according to the actual front wheel steering angle error at the current moment and the actual front wheel steering angle error change between two adjacent moments. It can also be determined by the following formula: α(e t ) is the first scaling factor, λ2 is the adjustment weight, 0<λ2<1, e is a natural constant, k3 is the adjustment coefficient, β(e ct ) is the second expansion factor, λ3 is the adjustment coefficient, 0<λ3<1, k4 is the adjustment coefficient, λ2, λ3, k3 and k4 can be previous experimental data or can be set based on experience:

[0073]

[0074]

[0075] In this embodiment, firstly, the accuracy of the angle correction parameter determined at the next moment can be further improved by the scaling factor, thereby making the steering amount of the motor on the target robot at the next moment finally determined more accurate, so as to further reduce the deviation between the actual driving path of the target robot and the preset driving path; secondly, on the basis of the conventional fuzzy PID controller, the idea of ​​variable domain fuzzy PID is introduced, and the steering amount of the motor on the target robot at the next moment is scaled and controlled by the actual front wheel angle error e(t) at the current moment and the actual front wheel angle error change ec(t) between the current moment and the previous moment, so that the system has better robustness.

[0076] In some embodiments, the actual front wheel steering angle error at each of two adjacent moments and the actual front wheel steering angle error change between two adjacent moments are determined according to the first mark point position, the second mark point position, the actual center of mass position at two adjacent moments, the actual heading angle at two adjacent moments, the actual front wheel steering angle at two adjacent moments, and a preset formula, including:

[0077] Determine the expected front wheel turning angle at the last moment according to the coordinates of the first marking point position, the coordinates of the second marking point position, the coordinates of the actual center of mass position at the last moment, and the actual heading angle at the last moment, and determine the actual front wheel turning angle error at the last moment according to the expected front wheel turning angle at the last moment and the actual front wheel turning angle at the last moment;

[0078] Determine the expected front wheel turning angle at the current moment according to the coordinates of the first marking point position, the coordinates of the second marking point position, the coordinates of the actual center of mass position at the current moment, and the actual heading angle at the current moment, and determine the actual front wheel turning angle error at the current moment according to the expected front wheel turning angle at the current moment and the actual front wheel turning angle at the current moment;

[0079] The actual front wheel steering angle error change between two adjacent moments is determined based on the actual front wheel steering angle error at the previous moment and the actual front wheel steering angle error at the current moment.

[0080] Here, the actual front wheel turning angle at the previous moment and the actual front wheel turning angle at the current moment can be obtained through devices such as angle sensors.

[0081] In this embodiment, the actual front wheel steering angle error at the previous moment is determined by the data at the previous moment, and the actual front wheel steering angle error at the current moment is determined by the data at the current moment, and then the actual front wheel steering angle error change between two adjacent moments is determined, so that the determined actual front wheel steering angle error change is more accurate.

[0082] In some embodiments of the present application, the expected turning angle of the front wheel at a certain moment can be determined according to the coordinates of the first marking point position, the coordinates of the second marking point position, the coordinates of the actual center of mass position at a certain moment, the actual heading angle at a certain moment, and the following formula:

[0083]

[0084]

[0085] d θ =(1-λ1)k1e d +λ1k2e θ

[0086] Among them, e d is the lateral deviation at a certain moment, (x1, y1) is the coordinate of the first marking point, (x2, y2) is the coordinate of the second marking point, (x3, y3) is the coordinate of the actual center of mass at a certain moment, e θ is the heading angle deviation at a certain moment, θ is the actual heading angle at a certain moment, d θ is the expected turning angle of the front wheel at a certain moment, λ1 is the deviation weight, k1 is the lateral deviation gain at a certain moment, and k2 is the heading angle deviation gain at a certain moment.

[0087] In other embodiments of the present application, the expected turning angle of the front wheel at a certain moment can be determined according to the coordinates of the first marking point position, the coordinates of the second marking point position, the coordinates of the actual center of mass position at a certain moment, the actual heading angle at a certain moment, and the following formula:

[0088]

[0089]

[0090] d θ =(1-λ1)e d +λ1e θ

[0091] Among them, e d is the lateral deviation at a certain moment, (x1, y1) is the coordinate of the first marking point, (x2, y2) is the coordinate of the second marking point, (x3, y3) is the coordinate of the actual center of mass at a certain moment, e θ is the heading angle deviation at a certain moment, θ is the actual heading angle at a certain moment, d θ is the expected turning angle of the front wheel at a certain moment, and λ1 is the deviation weight.

[0092] In some embodiments, the actual front wheel steering angle error at the last moment is determined according to the expected front wheel steering angle at the last moment, the actual front wheel steering angle at the last moment, and the following formula:

[0093] e(t-1)=d θ (t-1)-m θ (t-1)

[0094] The actual front wheel steering angle error at the current moment is determined based on the expected front wheel steering angle at the current moment, the actual front wheel steering angle at the current moment, and the following formula:

[0095] e(t)=d θ (t)-m θ (t)

[0096] The actual front wheel steering angle error change between two adjacent moments is determined based on the actual front wheel steering angle error at the previous moment, the actual front wheel steering angle error at the current moment, and the following formula:

[0097] ec(t)=e(t)-e(t-1)

[0098] Among them, e(t-1) is the actual front wheel steering angle error at the previous moment, d θ (t-1) is the expected turning angle of the front wheel at the previous moment, m θ (t-1) is the actual front wheel steering angle at the previous moment, e(t) is the actual front wheel steering angle error at the current moment, d θ (t) is the expected turning angle of the front wheel at the current moment, m θ (t) is the actual front wheel steering angle at the current moment, and ec(t) is the actual front wheel steering angle error change between two adjacent moments.

[0099] In some embodiments, the angle correction parameter at the next moment is determined according to the actual front wheel angle error at the current moment, the change of the actual front wheel angle error between two adjacent moments, and the variable domain fuzzy rule, and the steering amount of the motor on the target robot at the next moment is determined according to the actual front wheel angle error at the current moment, the actual front wheel angle error at the previous moment, and the angle correction parameter at the next moment to adjust the heading of the target robot, including:

[0100] Determine respectively the basic domains of the actual front wheel steering angle error at the current moment, the actual front wheel steering angle error change between two adjacent moments, the first steering angle parameter, the second steering angle parameter and the third steering angle parameter;

[0101] Determine a first telescoping factor and a second telescoping factor according to an actual front wheel steering angle error at a current moment and a change in the actual front wheel steering angle error between two adjacent moments;

[0102] Determine the first steering angle correction parameter, the second steering angle correction parameter and the third steering angle correction parameter in the steering angle correction parameter at the next moment respectively according to the basic domain of the actual front wheel steering angle error at the current moment, the basic domain of the actual front wheel steering angle error change between two adjacent moments, the basic domain of the first steering angle parameter, the basic domain of the second steering angle parameter, the basic domain of the third steering angle parameter, the first telescoping factor, the second telescoping factor and the variable domain fuzzy rule;

[0103] The steering amount of the motor on the target robot at the next moment is determined according to the actual front wheel angle error at the current moment, the actual front wheel angle error change between two adjacent moments, the first angle correction parameter, the second angle correction parameter and the third angle correction parameter.

[0104] In this embodiment, the first telescoping factor and the second telescoping factor are determined according to the actual front wheel angle error at the current moment and the change in the actual front wheel angle error between two adjacent moments, and the basic domain of the basic domain of the actual front wheel angle error at the current moment, the change in the actual front wheel angle error between two adjacent moments, the first angle parameter, the second angle parameter and the third angle parameter are adjusted according to the first telescoping factor and the second telescoping factor. That is, on the basis of the conventional fuzzy PID controller, the idea of ​​the variable domain fuzzy PID is introduced. The telescoping factor is based on the function-type variable domain fuzzy controller, which greatly improves the performance of resisting external interference, and solves the problem that when there is external interference in the conventional path tracking model, the PID algorithm will introduce external interference, resulting in instability of the control system, thereby more accurately controlling the movement of the agricultural robot.

[0105] In some embodiments, the first telescoping factor and the second telescoping factor are determined according to the actual front wheel steering angle error at the current moment, the actual front wheel steering angle error change between two adjacent moments, and the following formula:

[0106]

[0107]

[0108] The steering amount of the motor on the target robot at the next moment is determined according to the actual front wheel angle error at the current moment, the change in the actual front wheel angle error between two adjacent moments, the first angle correction parameter, the second angle correction parameter, the third angle correction parameter and the following formula:

[0109]

[0110] Among them, α(e t ) is the first scaling factor, λ2 is the adjustment weight, 0<λ2<1, e is a natural constant, k3 is the adjustment coefficient, β(e ct ) is the second telescopic factor, λ3 is the adjustment weight, 0<λ3<1, k4 is the adjustment coefficient, u is the steering amount of the motor on the target robot at the next moment, Δk P , Δk I and Δk D They are respectively the first corner correction parameter, the second corner correction parameter, and the third corner correction parameter.

[0111] In some embodiments, see Figures 2 to 4 As shown, the agricultural robot driving path control method is as follows:

[0112] Get the coordinates (x1, y1) of the first mark point position A in the preset driving path and the coordinates (x2, y2) of the second mark point position B in the preset driving path, and get the center of mass coordinates (x3, y3) of the target robot in the earth coordinate system at the current moment, the actual heading angle θ at the current moment, and the actual front wheel turning angle m at the current moment θ (t). AB is the smallest unit segment of the preset driving path, point A is the starting point of the smallest unit segment, point B is the end point of the smallest unit segment, and the actual front wheel steering angle is measured by the angle sensor.

[0113] According to the coordinates of the first mark point position A (x1, y1), the coordinates of the second mark point position B (x2, y2), the center of mass coordinates at the current moment (x3, y3) and the following formula, the lateral deviation e of the target robot at the current moment is determined: d (t):

[0114]

[0115] According to the actual heading angle θ of the target robot at the current moment, the coordinates of the first mark point position A (x1, y1), the coordinates of the second mark point position B (x2, y2) and the following formula, the heading angle deviation e of the target robot at the current moment is determined: θ (t):

[0116]

[0117]

[0118] According to the deviation weight λ1, the actual lateral deviation gain k1 at the current moment, the actual heading angle deviation gain k2 at the current moment, and the lateral deviation e of the target robot at the current moment d (t), the heading angle deviation e of the target robot at the current moment θ (t) and the following formula to determine the expected front wheel turning angle d at the current moment θ (t):

[0119] d θ (t) = (1-λ1)k1e d (t)+λ1k2e θ (t)

[0120] According to the expected turning angle d of the front wheel at the current moment θ (t), actual front wheel turning angle m θ (t) and the following formula to determine the actual front wheel steering angle error e(t) at the current moment:

[0121] e(t)=d θ (t)-m θ (t)

[0122] The actual front wheel steering angle error change ec(t) between the current moment and the previous moment is determined according to the actual front wheel steering angle error e(t) at the current moment, the actual front wheel steering angle error e(t-1) at the previous moment, and the following formula. The calculation method of the actual front wheel steering angle error e(t-1) at the previous moment is consistent with the calculation method of the actual front wheel steering angle error e(t) at the current moment in this embodiment, and will not be repeated here:

[0123] ec(t)=e(t)-e(t-1)

[0124] The first telescoping factor α(e) is determined according to the actual front wheel steering angle error e(t) at the current moment, the actual front wheel steering angle error change ec(t) between the current moment and the previous moment, the adjustment weights λ2 and λ3, 0<λ2<1, 0<λ3<1, the natural constant e, the adjustment coefficients k3 and k4, k3>0, k4>0 and the following formula t ) and the second expansion factor β(e ct );

[0125]

[0126]

[0127] like Figure 4As shown in FIG, according to the variable universe fuzzy PID algorithm (i.e., variable universe fuzzy rule), the upper-level planning controller inputs the preset path, and obtains the actual front wheel steering angle error e(t) at the current moment and the actual front wheel steering angle error change ec(t) between the current moment and the previous moment through data fusion of the expected steering angle. Then, the actual front wheel steering angle error e(t) at the current moment and the actual front wheel steering angle error change ec(t) between the current moment and the previous moment are used as the input signal of the variable universe fuzzy PID algorithm at moment t, and the first and second telescopic factors are output to the controller II after being adjusted by the controller I.

[0128] Assume that the basic domain of the actual front wheel steering angle error at the current moment is [-ε1,ε1], the basic domain of the actual front wheel steering angle error change between two adjacent moments is [-ε2,ε2], the basic domain of the first steering angle parameter is [-U1,U1], the basic domain of the second steering angle parameter is [-U2,U2], and the basic domain of the third steering angle parameter is [-U3,U3]. Here, the basic domain is determined based on experience.

[0129] According to the basic domain of the actual front wheel steering angle error at the current moment is [-ε1,ε1], the basic domain of the actual front wheel steering angle error change between two adjacent moments is [-ε2,ε2], the basic domain of the first steering angle parameter is [-U1,U1], the basic domain of the second steering angle parameter is [-U2,U2], the basic domain of the third steering angle parameter is [-U3,U3], the first expansion factor α(e t ) and the second stretch factor β(e ct ) Determine the adjustment domain E of the actual front wheel steering angle error at the current moment t , the adjustment domain E of the actual front wheel steering angle error change between two adjacent moments ct , the adjustment domain k of the first corner parameter p , the adjustment domain k of the second corner parameter i , the adjustment domain k of the third corner parameter d ;

[0130] E t =[-α(e t )ε1,α(e t )ε1]

[0131] E ct =[-α(e t )ε2,α(e t )ε2]

[0132] k p =[-β(e ct )U1,β(e ct )U1]

[0133] ki =[-β(e ct )U2,β(e ct )U2]

[0134] k d =[-β(e ct )U3,β(e ct )U3]

[0135] Set the adjustment domain E of the actual front wheel steering angle error at the current moment t , the adjustment domain E of the actual front wheel steering angle error change between two adjacent moments ct , the adjustment domain k of the first corner parameter p , the adjustment domain k of the second corner parameter i , the adjustment domain k of the third corner parameter d The fuzzy subsets are all {NB NM NS Z PS PM PB}, [NB] negative large, [NM] negative medium, [NS] negative small, [ZO] zero, [PS] positive small, [PM] positive medium, [PB] positive large. The continuously changing quantities within the adjustment domain are graded and discretized, and then fuzzy processed. The triangular function is used to divide the domain into its membership function, and the membership degree is obtained according to the membership function. The fuzzy rules are as follows: Figure 3 As shown. For example, the adjustment domain E of the actual front wheel steering angle error e(t) at the current moment is t [-3,3], the adjustment domain E of the actual front wheel steering angle error change ec(t) between two adjacent moments ct [-3,3], then Figure 3 In the equation, NB=-3,NM=-2,NS=-1,Z=0,PS=1,PM=2,PB=3,and e(t)=-3,ec(t)=-3, then the intersection point is [PB, NB, PS], and then in the adjustment domain k of the first corner parameter p The corresponding PB value in the fuzzy subset is the first rotation angle correction parameter Δk P , in the adjustment domain k of the second corner parameter I The corresponding value of NB in ​​the fuzzy subset is the second rotation angle correction parameter Δk I , in the adjustment domain k of the third corner parameter D The corresponding value of PS in the fuzzy subset is the third rotation angle correction parameter Δk D .

[0136] After the controller II fuzzy algorithm, the output Δk P , Δk I , Δk D , and then enters the PID controller to determine the steering amount u of the motor on the target robot at the next moment through the following formula.

[0137]

[0138] At the same time, the output is limited. When the integral term is saturated, the error is still accumulated under the integral action. Once the error starts to change in the opposite direction, the system needs time to exit the saturation zone. Therefore, when the output reaches the minimum and maximum, the integral action should be stopped, and the integral and output limits should be performed. δ, is the limit boundary.

[0139]

[0140]

[0141] The judgment result is adjusted by the output expansion factor to obtain the correction parameter ΔK P , ΔK I , ΔK D The digital value of the steering speed of the steering motor is input into the PID controller to obtain the working condition parameter output under the working state.

[0142] For example, the orchard fertilization robot receives the expected planning path planned by the upper controller in the orchard, and makes a difference with the actual route measured by the sensor to obtain the lateral deviation and heading angle deviation. In view of the shortcomings of PID controller such as strong dependence on the model, difficulty in parameter setting, and small dynamic adjustment range, the variable domain idea is introduced on the basis of the gain-adjusted fuzzy PID controller. According to the e of each stage of the system response, the variable domain of the system is calculated. t 、e ct Based on the running trajectory, an adaptive adjustment mechanism of the domain scaling factor is designed to adjust the mapping relationship between the input and output variables and fuzzy subsets at each stage of the system operation online, so as to realize the scaling of the domain as the control requirements of the system, improve the dynamic characteristics and steady-state accuracy of the system, and enhance the dynamic adjustment capability of the system, ultimately achieving the progress of the robot's trajectory tracking and the stability of the robot's driving.

[0143] In this embodiment, on the basis of the conventional fuzzy PID controller, the idea of ​​variable universe fuzzy PID is introduced. The variable universe fuzzy controller based on the function type of the scaling factor greatly improves the performance of resisting external interference, and solves the problem that the PID algorithm of the conventional path tracking model will introduce external interference and cause the control system to be unstable when there is external interference. Therefore, the movement of agricultural robots such as the orchard intelligent fertilizer cart is more accurately controlled. The variable universe adaptive fuzzy control is used to realize stable adaptive control of nonlinear systems such as orchard robots, which has good stability and robustness, so as to accurately achieve the control objectives.

[0144] Second aspect

[0145] Based on the same inventive concept, see Figure 4 As shown, the present application provides a driving path control system for an agricultural robot, using any of the above methods. The system includes: a data acquisition module, a data fusion module and a data output module. The data acquisition module is used to obtain the first mark point position and the second mark point position in the preset driving path of the target robot, and respectively obtain the actual center of mass position, actual heading angle and actual front wheel turning angle of the target robot at two adjacent moments, and the two adjacent moments are: the current moment and the previous moment. The data fusion module is used to determine the actual front wheel turning angle error of each moment in the two adjacent moments and the actual front wheel turning angle error change between the two adjacent moments according to the first mark point position, the second mark point position, the actual center of mass position of the two adjacent moments, the actual heading angle of the two adjacent moments and the actual front wheel turning angle of the two adjacent moments. The data output module is used to determine the angle correction parameter of the next moment according to the actual front wheel turning angle error of the current moment, the actual front wheel turning angle error change between the two adjacent moments and the variable domain fuzzy rule, and determine the steering amount of the motor on the target robot at the next moment according to the angle correction parameter of the next moment to adjust the heading of the target robot.

[0146] In this embodiment, the steering amount of the motor on the target robot at the next moment is determined by combining the actual front wheel angle error at two adjacent moments and the angle correction parameter at the next moment, so that the steering amount of the motor on the target robot at the next moment can be adjusted in real time according to the front wheel angle error in real time, so as to reduce the deviation between the actual driving path and the preset driving path. Secondly, on the basis of the conventional fuzzy PID controller, the idea of ​​variable domain fuzzy PID is introduced, which greatly improves the performance of resisting external interference and solves the problem that the PID algorithm of the conventional path tracking model will introduce external interference and cause the control system to be unstable when there is external interference, thereby more accurately controlling the movement of the agricultural robot.

[0147] It should be noted here that the description of the above system embodiment is similar to the description of the above method embodiment, and has similar beneficial effects as the method embodiment. For technical details not disclosed in the system embodiment of the present application, please refer to the description of the method embodiment of the present application for understanding.

[0148] The third aspect

[0149] Based on the same inventive concept, an embodiment of the present application provides an electronic device, which includes: a processor, a memory, and a bus; wherein the processor and the memory communicate with each other through the bus; and the processor is used to call program instructions in the memory to execute any of the methods described above.

[0150] It should be noted that the description of the above electronic device embodiment is similar to the description of the above method embodiment, and has similar beneficial effects as the method embodiment. For technical details not disclosed in the electronic device embodiment of this application, please refer to the description of the method embodiment of this application for understanding.

[0151] The fourth aspect

[0152] Based on the same inventive concept, an embodiment of the present application provides a computer-readable storage medium, the computer-readable storage medium comprising: a stored program; wherein, when the program is running, the device where the storage medium is located is controlled to execute any of the methods described above.

[0153] It should be noted here that the description of the above storage medium embodiment is similar to the description of the above method embodiment, and has similar beneficial effects as the method embodiment. For technical details not disclosed in the storage medium embodiment of the present application, please refer to the description of the method embodiment of the present application for understanding.

[0154] The above is only a specific implementation of the present application, but the protection scope of the present application is not limited thereto. Any person skilled in the art who is familiar with the present technical field can easily think of changes or substitutions within the technical scope disclosed in the present application, which should be included in the protection scope of the present application. Therefore, the protection scope of the present application should be based on the protection scope of the claims.

Claims

1. A method for controlling a driving path of an agricultural robot, characterized in that: include: Obtain the first mark point position and the second mark point position in the preset driving path of the target robot, and respectively obtain the actual center of mass position, actual heading angle and actual front wheel turning angle of the target robot at each of two adjacent moments, wherein the two adjacent moments are respectively: the current moment and the previous moment; According to the first marking point position, the second marking point position, the actual center of mass position at the two adjacent moments, the actual heading angle at the two adjacent moments, the actual front wheel turning angle at the two adjacent moments and a preset formula, respectively determine the actual front wheel turning angle error at each of the two adjacent moments and the actual front wheel turning angle error change between the two adjacent moments; Determine the steering angle correction parameter at the next moment according to the actual front wheel steering angle error at the current moment, the change of the actual front wheel steering angle error between the two adjacent moments and the variable domain fuzzy rule, and determine the steering amount of the motor on the target robot at the next moment according to the actual front wheel steering angle error at the current moment, the actual front wheel steering angle error at the previous moment and the steering angle correction parameter at the next moment to adjust the heading of the target robot; It includes: respectively determining the basic domains of the actual front wheel steering angle error at the current moment, the actual front wheel steering angle error change between two adjacent moments, the first steering angle parameter, the second steering angle parameter and the third steering angle parameter; The first telescoping factor and the second telescoping factor are determined according to the actual front wheel steering angle error at the current moment and the actual front wheel steering angle error change between two adjacent moments and the following formula: Determine the first steering angle correction parameter, the second steering angle correction parameter, and the third steering angle correction parameter of the steering angle correction parameter at the next moment respectively according to the basic domain of the actual front wheel steering angle error at the current moment, the basic domain of the actual front wheel steering angle error change between two adjacent moments, the basic domain of the first steering angle parameter, the basic domain of the second steering angle parameter, the basic domain of the third steering angle parameter, the first telescoping factor, the second telescoping factor, and the variable domain fuzzy rule; According to the basic domain [-ε1, ε1] of the actual front wheel steering angle error at the current moment, the basic domain [-ε2, ε2] of the actual front wheel steering angle error change between two adjacent moments, the basic domain [-U1, U1] of the first steering angle parameter, the basic domain [-U2, U2] of the second steering angle parameter, the basic domain [-U3, U3] of the third steering angle parameter, the first telescoping factor α (e t ) and the second stretch factor β(e ct ) Determine the adjustment domain E of the actual front wheel steering angle error at the current moment t , the adjustment domain E of the actual front wheel steering angle error change between the two adjacent moments ct , the adjustment domain k of the first turning angle parameter p , the adjustment domain k of the second turning angle parameter i , the adjustment domain k of the third turning angle parameter d ; E t =[-α(e t )ε1,α(e t )e1] E ct =[-α(e t )ε2,α(e t )ε2] k p =[-β(e ct )U1,β(e ct )U1] k i =[-β(e ct )U2,β(e ct )U2] k d =[-β(e ct )U3,β(e ct )U3] Set the adjustment domain E of the actual front wheel steering angle error at the current moment t , the adjustment domain E of the actual front wheel steering angle error change between the two adjacent moments ct , the adjustment domain k of the first turning angle parameter p , the adjustment domain k of the second turning angle parameter i , the adjustment domain k of the third turning angle parameter d The fuzzy subsets are all {NB NM NS Z PS PM PB}, [NB] negative large, [NM] negative medium, [NS] negative small, [ZO] zero, [PS] positive small, [PM] positive medium, [PB] positive large. The continuously changing quantities within the adjustment domain are graded and discretized, and then fuzzy processed. The triangular function is used to divide the domain into its membership function, and the membership degree is obtained according to the membership function. The fuzzy rules are as follows: In the adjustment domain k of the first corner parameter p The corresponding PB value in the fuzzy subset is the first rotation angle correction parameter Δk P , in the adjustment domain k of the second corner parameter I The corresponding value of NB in ​​the fuzzy subset is the second rotation angle correction parameter Δk I , in the adjustment domain k of the third corner parameter D The corresponding value of PS in the fuzzy subset is the third rotation angle correction parameter Δk D ; The steering amount of the motor on the target robot at the next moment is determined according to the actual front wheel steering angle error at the current moment, the actual front wheel steering angle error change between the two adjacent moments, the first steering angle correction parameter, the second steering angle correction parameter, the third steering angle correction parameter and the following formula: Among them, α(e t ) is the first scaling factor, λ2 is the adjustment weight, 0<λ2<1, e is a natural constant, k3 is the adjustment coefficient, β(e ct ) is the second telescopic factor, λ3 is the adjustment weight, 0<λ3<1, k4 is the adjustment coefficient, u is the steering amount of the motor on the target robot at the next moment, e(t) is the actual front wheel steering angle error at the current moment, and e(t-1) is the actual front wheel steering angle error at the previous moment.

2. The agricultural robot driving path control method according to claim 1, characterized in that: The method of determining the actual front wheel steering angle error at each of the two adjacent moments and the actual front wheel steering angle error change between the two adjacent moments according to the first mark point position, the second mark point position, the actual center of mass position at the two adjacent moments, the actual heading angle at the two adjacent moments, and the actual front wheel steering angle at the two adjacent moments comprises: Determine the expected front wheel turning angle at the previous moment according to the coordinates of the first marking point position, the coordinates of the second marking point position, the coordinates of the actual center of mass position at the previous moment, and the actual heading angle at the previous moment, and determine the actual front wheel turning angle error at the previous moment according to the expected front wheel turning angle at the previous moment and the actual front wheel turning angle at the previous moment; Determine the expected front wheel turning angle at the current moment according to the coordinates of the first marking point position, the coordinates of the second marking point position, the coordinates of the actual center of mass position at the current moment, and the actual heading angle at the current moment, and determine the actual front wheel turning angle error at the current moment according to the expected front wheel turning angle at the current moment and the actual front wheel turning angle at the current moment; The actual front wheel steering angle error change between the two adjacent moments is determined according to the actual front wheel steering angle error at the previous moment and the actual front wheel steering angle error at the current moment.

3. The method for controlling the driving path of an agricultural robot according to claim 2, characterized in that: The expected turning angle of the front wheel at a certain moment is determined according to the coordinates of the first marking point position, the coordinates of the second marking point position, the coordinates of the actual center of mass position at a certain moment, the actual heading angle at a certain moment, and the following formula: d θ =(1-λ1)k1e d +λ1k2e θ Among them, e d is the lateral deviation at a certain moment, (x1, y1) is the coordinate of the first marking point, (x2, y2) is the coordinate of the second marking point, (x3, y3) is the coordinate of the actual center of mass at a certain moment, e θ is the heading angle deviation at a certain moment, θ is the actual heading angle at a certain moment, d θ is the expected turning angle of the front wheel at a certain moment, λ1 is the deviation weight, k1 is the lateral deviation gain at a certain moment, and k2 is the heading angle deviation gain at a certain moment.

4. The method for controlling the driving path of an agricultural robot according to claim 3, characterized in that: The actual front wheel turning angle error at the previous moment is determined according to the expected front wheel turning angle at the previous moment, the actual front wheel turning angle at the previous moment and the following formula: e(t-1)=d θ (t-1)-m θ (t-1) The actual front wheel turning angle error at the current moment is determined according to the expected front wheel turning angle at the current moment, the actual front wheel turning angle at the current moment and the following formula: e(t)=d θ (t)-m θ (t) The actual front wheel steering angle error change between the two adjacent moments is determined according to the actual front wheel steering angle error at the previous moment, the actual front wheel steering angle error at the current moment and the following formula: ec(t)=e(t)-e(t-1) Among them, e(t-1) is the actual front wheel steering angle error at the previous moment, d θ (t-1) is the expected turning angle of the front wheel at the previous moment, m θ (t-1) is the actual front wheel steering angle at the previous moment, e(t) is the actual front wheel steering angle error at the current moment, d θ (t) is the expected turning angle of the front wheel at the current moment, m θ (t) is the actual front wheel steering angle at the current moment, and ec(t) is the actual front wheel steering angle error change between two adjacent moments.

5. A driving path control system for an agricultural robot, using the method according to any one of claims 1 to 4, characterized in that: The system comprises: A data acquisition module, the data acquisition module is used to obtain the first mark point position and the second mark point position in the preset driving path of the target robot, and respectively obtain the actual center of mass position, actual heading angle and actual front wheel turning angle of the target robot at two adjacent moments, the two adjacent moments are respectively: the current moment and the previous moment; a data fusion module, the data fusion module being used to respectively determine an actual front wheel steering angle error at each of the two adjacent moments and a change in the actual front wheel steering angle error between the two adjacent moments according to the first marking point position, the second marking point position, the actual centroid position at the two adjacent moments, the actual heading angle at the two adjacent moments, and the actual front wheel steering angle at the two adjacent moments; A data output module, the data output module is used to determine the angle correction parameter at the next moment according to the actual front wheel angle error at the current moment, the change in the actual front wheel angle error between the two adjacent moments and the variable domain fuzzy rule, and determine the steering amount of the motor on the target robot at the next moment according to the angle correction parameter at the next moment to adjust the heading of the target robot.

6. An electronic device, characterized in that: include: Processor, memory, bus; Wherein, the processor and the memory communicate with each other through the bus; The processor is used to call the program instructions in the memory to execute the method according to any one of claims 1 to 4.

7. A computer-readable storage medium, characterized in that: include: stored programs; Wherein, when the program is running, the device where the storage medium is located is controlled to execute the method as claimed in any one of claims 1 to 4.

Citation Information

Patent Citations

  • Mechanical automatic steering control method

    CN101221447A

  • Cascade variable universe fuzzy PID attitude control system and method of four-rotor aircraft

    CN110333733A

  • Tractor paddy field operation path tracking method and system based on variable universe fuzzy PID theory

    CN113985729A

  • Control method, apparatus and system for robot, and computer-readable storage medium

    WO2018223776A1