Disclosure of Invention
Aiming at the defects in the prior art, the invention aims to overcome the defects in the prior art, solve the technical problems that the obstacle avoidance path is difficult to control and the control precision is low in the prior art, and provide a field obstacle avoidance path planning and control method for unmanned agricultural machinery.
The purpose of the invention is realized as follows: a method for planning the path of agricultural machine to avoid obstacle in field includes such steps as automatically bypassing obstacle,
step 1: acquiring environmental information of the agricultural machinery through a sensor to make an obstacle avoidance decision;
step 2: calculating a theoretical obstacle avoidance path off line by using an improved shortest tangent method;
and step 3: and (3) optimizing the theoretical obstacle avoidance path in the step (2) by using a path optimization method based on a Bezier curve to obtain an actual obstacle avoidance path, so that the optimized path is easier to control, and controlling the front wheel corner of the agricultural machine by using the combination of the preview and the PI controller to enable the agricultural machine to walk along the actual obstacle avoidance path to avoid the obstacle.
When the agricultural machine is in work, a sensor arranged on the agricultural machine senses environmental information around the agricultural machine in the walking process of the agricultural machine, when an obstacle is in front of the agricultural machine, an obstacle avoidance decision is made, a theoretical obstacle avoidance path is calculated by using an improved shortest tangent method, the theoretical obstacle avoidance path is optimized to obtain an actual obstacle avoidance path which is easier to control, the agricultural machine obtains position information of the agricultural machine through detection of the sensor, a curve curvature, course deviation and transverse deviation of a set path are obtained in real time by using a curve tracking method, a current front wheel steering angle is calculated by combining a state feedback controller and an adaptive controller, and the agricultural machine enables the agricultural machine to walk along the set actual obstacle avoidance path by adjusting the front wheel steering angle of the agricultural machine in real time in the walking process, so that automatic obstacle avoidance of the agricultural machine is realized; according to the method, a theoretical obstacle avoidance path is calculated through an improved shortest tangent method, the theoretical obstacle avoidance path is optimized through a path optimization method based on a Bezier curve, the obstacle avoidance path is easier to control, the front wheel steering angle of the agricultural machine is controlled through the combination of a state feedback controller and a self-adaptive controller, the agricultural machine is enabled to walk along the set obstacle avoidance curve, and the control precision is high; the device can be applied to the automatic obstacle avoidance work of unmanned agricultural machinery during field operation.
In order to further improve the reliability of obtaining the theoretical obstacle avoidance path, in the step 2, the theoretical obstacle avoidance path is calculated, specifically, the size of a characteristic circle of an obstacle in front of the agricultural machine and the distance between the agricultural machine and the obstacle are calculated, a safety distance is set according to the size of the characteristic circle, and a theoretical obstacle avoidance path is set according to the width of a plough of the agricultural machine and the minimum turning radius of the agricultural machine.
In order to make the obstacle avoidance path easier to control, in the step 2, the shortest tangent method specifically is to make a characteristic circle by taking the center of the obstacle as the center of the circle, and the radius of the characteristic circle is rmin+ w/2, the theoretical obstacle avoidance path is composed of a first arc section, a first straight line section, a second arc section, a second straight line section and a third arc section, one end of the first arc section is tangent to the original straight line path of the agricultural machine, the other end of the first arc section is tangent to one end of the first straight line section, the other end of the first straight line section and one end of the second straight line section are respectively tangent to the second arc section, the other end of the second straight line section is tangent to the third arc section, the second arc section is a section on a characteristic circle, the first arc section and the third arc section are symmetrically arranged relative to the central line of the second arc section, the agricultural machine sequentially passes through the first arc section, the first straight line section, the second arc section, the second straight line section andminthe radius of the circumscribed circle of the barrier is smaller than the minimum turning radius.
In order to further improve the precision of the turning path of the agricultural machinery, the radius of the first arc section is rminThe radius of the third arc segment is rminThe starting point of the first arc segment is marked as H point, and the circle center of the first arc segment is marked as O point1Point, the intersection point of the first straight line segment and the original straight line path of the agricultural machine is recorded as J, the tangent point of the first straight line segment and the second circular arc segment is recorded as D, the intersection point of the original path of the agricultural machine and the characteristic circle is respectively recorded as K and K', JK = w/2, the circle center of the second circular arc segment is recorded as O, the coordinate of O is set as (a, B), the center point of the second circular arc segment is recorded as B, the coordinate of the J point is recorded as (x 1, y 1), and the equation of JD can be written as:
（1-1）；
the equation for the characteristic circle can be written as:
（1-2）
k can be solved through (1-1) and (1-2), and the D point is the intersection point of JD and the characteristic circle, so that the coordinates of the D point are solved;
set point O1Has the coordinates of (x)2,y2) Then point O1The distance to the line JD is:
o is obtained from the equations (1-3) and (1-4)1The coordinates of (a); the coordinates of the point H are (x)2，y1) The coordinates of the point B are (a, B + r);
the design establishes the relation of mathematical relations for each line segment forming the theoretical obstacle avoidance path, determines the specific shape of a curve, and solves the coordinates of the bending point, thereby facilitating the next optimization of the theoretical obstacle avoidance path.
In order to optimize the theoretical obstacle avoidance path designed by the improved shortest tangent method in the invention, in step 3, the theoretical obstacle avoidance path in step 2 is optimized by a path optimization method based on Bezier curve, specifically, a Bezier equation is established,
(1) position vector of given space n +1 pointThen, the interpolation formula of the coordinates of each point on the parameter curve is:
（2-1）
whereinThe characteristic points that make up the curve are,is the Bernstein basis function n times:
（2-2）
from the above formula, a mathematical expression of cubic and quadratic Bezier curves can be obtained, where when n =3, q (t) is a cubic polynomial, with four control points, expressed in matrix form as:
（2-3）
when n =2, q (t) is a quadratic polynomial, there are three control points, and the matrix expression is:
（2-4）
(2) the curvature expression of the Bezier curve is:
（2-5）
where y = f (x) represents the equation for the curve, y' is the first derivative of the curve, and y "is the second derivative;
the curvature radius is:
（2-6）；
in the design, a Bezier curve optimization method is provided for optimizing a theoretical obstacle avoidance path, specifically, the theoretical obstacle avoidance path with discontinuous curvature is optimized into an actual obstacle avoidance path with continuous curvature, and the actual obstacle avoidance path is easier to control.
In order to improve the controllability of the Bezier curve, selecting a cubic Bezier curve, aiming at the cubic Bezier curve:
（2-7）
（2-8）
wherein, X0, X1, X2 and X3 are respectively transverse coordinates at a point P0, a point P1, a point P2 and a point P3, Y0, Y1, Y2 and Y3 are respectively longitudinal coordinates at a point P0, a point P1, a point P2 and a point P3;
the point P0 corresponds to the starting point H (x) of the first arc segment2，y1) The point P3 corresponds to the center points B (a, B + r) and P1 of the second arc segment ((x)2+a）/2，y1) Point P2 ((x)2+ a)/2, b + r), the curvature radius calculation formula of the curve corresponding to the actual fault path is:
（2-9）；
in the design, a bending point on a theoretical obstacle avoidance path is selected as an optimization point in the Betizer curve optimization method, the optimized path is simpler, the curvature is continuous, and the control is easy.
In order to further improve the accuracy of obtaining the kinematic parameters of the agricultural machine, in step 3, the agricultural machine is simplified into a two-wheel vehicle model for kinematic analysis, and a curve tracking method is used for establishing the kinematic model of the agricultural machine, which is shown as the following formula:
（3-1）
wherein, s represents the distance of the M point moving along the arc length, and the M point is the closest point to the center of the rear axle of the agricultural machine on the curve path; y represents the transverse deviation between the agricultural machine and the M point, and theta is the heading deviation angle of the agricultural machine and is the steering angular acceleration; when the point moves clockwise along the curve, the curvature c is negative, and moves anticlockwise along the curve, the curvature c is positive; when the central point of the rear shaft of the agricultural machine is positioned on the outer side of the curve, the transverse deviation y is positive, and when the central point of the rear shaft of the agricultural machine is positioned on the inner side of the curve, the transverse deviation y is negative;
firstly, converting a nonlinear model of the agricultural machine into an approximate linear model by using a chain control theory, and then calculating a control rate by using a state feedback control method, wherein when the agricultural machine moves along a curve anticlockwise, the control rate is as follows:
（3-2）
when the agricultural machinery moves clockwise along the curve, the control rate is as follows:
（3-3）
wherein, the function of the steering angle of the theoretical front wheel is related to the transverse deviation and the agricultural machine course deviation angle variable;
in the design, an agricultural machinery motion model is established by using a curve tracking method, then a nonlinear agricultural machinery model is converted into an approximate linear model by using a chain control theory, kinematic parameters can be obtained in real time, the aim of controlling the actual walking path of the agricultural machinery is fulfilled by controlling the steering angle of the front wheel, and the agricultural machinery motion control system is small in error and high in control precision.
As a further improvement of the present invention, the adaptive controller has 2 input variables, namely a lateral deviation variable y and a lateral deviation differential dy, and the output of the adaptive controller is the desired steering compensation angle c of the agricultural machine, wherein the variable dy is the trend of the lateral deviation of the agricultural machine, and the calculation formula of dy is as follows:
（4）
wherein, ytSelecting a parameter △ t as 1s, wherein the transverse deviation is the transverse deviation of the agricultural machinery at the moment t;
fuzzification of input and output variables
(1) Transverse deviation y
Basic domain of discourse: [ -60,60], quantization scale: { -6, -5, -4, -3, -2, -1,0,1,2,3,4,5,6}, with a quantization factor Ky =6/60= 0.1;
(2) lateral deviation differential dy
Basic domain of discourse: [ -6,6], quantization scale: { -6, -5, -4, -3, -2, -1,0,1,2,3,4,5,6}, with a quantization factor Ky =6/6= 1;
(3) compensation angle c
Basic discourse area [ -8,8], quantization scale: { -6, -5, -4, -3, -2, -1,0,1,2,3,4,5,6}, quantization factor Ky =6/8= 0.75;
the blur levels of the lateral deviation y, the lateral deviation differential dy and the compensation angle c 3 variables are: negative Big (NB), Negative Middle (NM), Negative Small (NS), Zero (ZO), Positive Small (PS), Positive Middle (PM) and Positive Big (PB), wherein the three membership functions all adopt Gaussian functions.
In order to further improve the control accuracy of the tracking curve and the control accuracy of curve tracking, in step 3, after a theoretical front wheel steering angle (y, theta) is calculated by using a chain control theory, an expected steering compensation angle c is calculated by using an adaptive controller, an actual steering angle e of the agricultural machine is obtained after the theoretical front wheel steering angle (y, theta) and the expected steering compensation angle c are added and is output to an agricultural machine model, and the actual steering angle e of the agricultural machine is controlled to enable the agricultural machine to walk along a set curve; in the design, considering that the model accuracy of the agricultural machine has a large influence on the control quality of the agricultural machine, in the formula (3-1), we consider that c (x) vssin θ dt is approximately 0, and ds is approximately dx, when the curvature of the tracking curve is small, the assumed condition can be basically met, along with the increase of the curvature, the accuracy of the agricultural machine model will be reduced, the control performance will be reduced, and the auxiliary control is performed after the expected steering compensation angle c is added, so that the control accuracy of the agricultural machine is further improved.
As a further improvement of the present invention, the sensors include a position sensor, an angle sensor and a machine vision camera, the angle sensor detects a steering angle of the agricultural machine, the position sensor obtains position information of the agricultural machine; the vision machine camera is equipped with 2 and sets up respectively in the front and back side of agricultural machinery, and the vision machine camera acquires the geographic information of the environment that agricultural machinery is located.
Detailed Description
The invention will be further described with reference to the accompanying drawings.
As shown in fig. 1-11, a method for planning and controlling an unmanned field obstacle avoidance path of an agricultural machine comprises the steps of automatically bypassing obstacles,
step 1: acquiring environmental information of the agricultural machinery through a sensor to make an obstacle avoidance decision;
step 2: calculating a theoretical obstacle avoidance path off line by using an improved shortest tangent method;
and step 3: optimizing the theoretical obstacle avoidance path in the step 2 by using a path optimization method based on a Bezier curve to obtain an actual obstacle avoidance path, obtaining a real-time curve curvature and real-time heading deviation and transverse deviation of the agricultural machine by using a curve tracking method, calculating a current front wheel steering angle by using the combination of a state feedback controller and an adaptive controller, and controlling the steering angle of the agricultural machine to enable the agricultural machine to walk along the actual obstacle avoidance path so as to bypass obstacles and return to the original navigation path;
in step 1 of the invention, a sensor comprises a position sensor, an angle sensor and a machine vision camera, wherein the angle sensor detects the steering angle of the agricultural machine, and the position sensor obtains the position information of the agricultural machine; the number of the vision machine cameras is 2, the vision machine cameras are respectively arranged on the front side and the rear side of the agricultural machinery, and the vision machine cameras acquire geographic information of the environment where the agricultural machinery is located;
in the step 2, the theoretical obstacle avoidance path is calculated, specifically, the size of a characteristic circle of an obstacle in front of the agricultural machine and the distance between the agricultural machine and the obstacle are calculated, a safety distance is set according to the size of the characteristic circle, and a theoretical obstacle avoidance path is set according to the width of a plough of the agricultural machine and the minimum turning radius of the agricultural machine.
In order to make the obstacle avoidance path easier to control, in the step 2, the shortest tangent method specifically is to make a characteristic circle by taking the center of the obstacle as the center of the circle, and the radius of the characteristic circle is rmin+ w/2, as shown in fig. 1, the theoretical obstacle avoidance path is composed of a first arc segment, a first straight line segment, a second arc segment, a second straight line segment and a third arc segment, one end of the first arc segment is tangent to the original straight line path of the agricultural machine, the other end of the first arc segment is tangent to one end of the first straight line segment, the other end of the first straight line segment and one end of the second straight line segment are respectively tangent to the second arc segment, the other end of the second straight line segment is tangent to the third arc segment, the second arc segment is a segment on a characteristic circle, the first arc segment and the third arc segment are symmetrically arranged about a central line of the second arc segment, the agricultural machine sequentially passes through the first arc segment, the first straight line segment, the second arc segment, the second straight line segmentminThe radius of a circumscribed circle of the obstacle is smaller than the minimum turning radius;
the radius of the first arc segment is rminThe radius of the third arc segment is rminThe starting point of the first arc segment is marked as H point, and the circle center of the first arc segment is marked as O point1Point, the intersection point of the first straight line segment and the original straight line path of the agricultural machine is recorded as J, the tangent point of the first straight line segment and the second circular arc segment is recorded as D, the intersection point of the original path of the agricultural machine and the characteristic circle is respectively recorded as K and K', JK = w/2, the circle center of the second circular arc segment is recorded as O, the coordinate of O is set as (a, B), the center point of the second circular arc segment is recorded as B, the coordinate of the J point is recorded as (x 1, y 1), and the equation of JD can be written as:
（1-1）；
the equation for the characteristic circle can be written as:
（1-2）
k can be solved through (1-1) and (1-2), and the D point is the intersection point of JD and the characteristic circle, so that the coordinates of the D point are solved;
set point O1Has the coordinates of (x)2,y2) Then point O1The distance to the line JD is:
o is obtained from the equations (1-3) and (1-4)1The coordinates of (a); the coordinates of the point H are (x)2，y1) The coordinates of the point B are (a, B + r);
in order to optimize the theoretical obstacle avoidance path designed by the improved shortest tangent method in the invention, in step 3, the theoretical obstacle avoidance path in step 2 is optimized by a path optimization method based on Bezier curve, specifically, a Bezier equation is established,
(1) given the position vector of n +1 points in the space, the interpolation formula of the coordinates of each point on the parameter curve is as follows:
（2-1）
whereinThe characteristic points that make up the curve are,is the Bernstein basis function n times:
（2-2）
from the above formula, a mathematical expression of cubic and quadratic Bezier curves can be obtained, where when n =3, q (t) is a cubic polynomial, with four control points, expressed in matrix form as:
（2-3）
when n =2, q (t) is a quadratic polynomial, there are three control points, and the matrix expression is:
（2-4）
(2) properties of Bezier curves
Obtaining the values of two end points of the Bezier curve by the formula (2-1):
when the t =0, the signal is transmitted,
（2-10）
when the t =1, the signal strength of the signal is high,
（2-11）
the derivative function of the Bezier curve is found for equation (2-1) as:
（2-12）
at the time of the start point t =0,
（2-13）
at the time of the starting point t =1,
（2-14）
quadratic Bezier curve endpoint properties:
（2-15）
the cubic Bezier curve end point properties are:
（2-16）
from the analysis of the properties of the Bezier curve, the tangential directions at the starting point and the ending point are consistent with the trends of the first edge and the last edge of the characteristic polygon, and the determination of the initial pose and the target pose of the vehicle is realized by planning the tangential directions of the starting point and the ending point of the Bezier curve; as can be seen from FIGS. 2 and 3, the cubic Bezier curves all fall within the feature polygon P0P1P2P3, increasing the controllability of the Bezier curves;
(3) the curvature expression of the Bezier curve is:
（2-5）
where y = f (x) represents the equation for the curve, y' is the first derivative of the curve, and y "is the second derivative;
the curvature radius is:
（2-6）；
aiming at the analysis, in order to improve the controllability of the Bezier curve, the invention selects the cubic Bezier curve, aiming at the cubic Bezier curve:
（2-7）
（2-8）
wherein, X0, X1, X2 and X3 are respectively transverse coordinates at a point P0, a point P1, a point P2 and a point P3, Y0, Y1, Y2 and Y3 are respectively longitudinal coordinates at a point P0, a point P1, a point P2 and a point P3;
the point P0 corresponds to the starting point H (x) of the first arc segment2，y1) The point P3 corresponds to the center points B (a, B + r) and P1 of the second arc segment ((x)2+a）/2，y1) Point P2 ((x)2+ a)/2, b + r), the curvature radius calculation formula of the curve corresponding to the actual fault path is:
（2-9）；
an actual obstacle avoidance path formed after a theoretical obstacle avoidance path is optimized by a Bezier curve optimization method is shown in FIG. 4, the curvature of the actual obstacle avoidance path is simulated by matlab software, and as can be seen from FIG. 5, the curvature of the actual obstacle avoidance path is continuous, and the agricultural machinery is easy to control to walk along the curve;
in order to improve the control precision of the tracking curve, in step 3, the agricultural machinery is simplified into a two-wheel vehicle model for kinematic analysis, and a curve tracking method is used for establishing an agricultural machinery kinematic model (as shown in fig. 7), which is shown as the following formula:
（3-1）
wherein, s represents the distance of the M point moving along the arc length, and the M point is the closest point to the center of the rear axle of the agricultural machine on the curve path; y represents the transverse deviation between the agricultural machine and the M point, and theta is the heading deviation angle of the agricultural machine and is the steering angular acceleration; when the point moves clockwise along the curve, the curvature c is negative, and moves anticlockwise along the curve, the curvature c is positive; when the central point of the rear shaft of the agricultural machine is positioned on the outer side of the curve, the transverse deviation y is positive, and when the central point of the rear shaft of the agricultural machine is positioned on the inner side of the curve, the transverse deviation y is negative;
the model of the agricultural machinery is known to be a highly nonlinear system from (3-1), in order to apply a linear system control method, (3-2) needs to be approximately linearized, a chain control theory can be used for converting the agricultural machinery model into an approximately linear model, and the method can enable us to apply the linear control method to the nonlinear system;
converting the non-linear model of the agricultural machine into a universal chain system, wherein the equation of the universal third-order chain system is as follows:
（3-4）
wherein,is a state variable of the system and is,is a control variable of the system, and in order to linearize equation (3-1), the state variable a1 needs to be derived and recorded
(3-5)
Equation (3-5) can be written as follows:
(3-6)
the last two equations (3-5) and (3-6) in the system (3-4) are obviously linear systems, so we can also deduce that there are n-1 linear subsystems in the chain system of n dimension;
next, the agricultural machine model (3-1) is converted into a chain system form, setting a1= s, while the state variables a2 and a3 are set as y and theta related variables, and we can select the simplest form, setting a2= y,
the new control variable m1 will be written as follows:
(3-7)
at the same time, set
(3-8)
Substituting the agricultural machinery model (3-1) into the formulas (3-7) and (3-8) can convert the agricultural machinery model (3-1) into the form of (3-5), and the derivation process is as follows:
(3-9)
among them, the following conditions must be satisfied:
(3-10)
since the models (3-8) are linear systems, we can control the system using a state feedback control method, and the general expression of the state feedback controller is as follows:
(3-11)
by substituting equation (3-11) into equation (3-8), we can obtain the following control law:
(3-12)
the control law is to control a2 and a3 to approach 0, and similarly, the conclusion can be used to control y and theta to approach 0 for the purpose of curve tracking control;
substituting equation (3-12) into equation (3-8) results in the actual control expression:
converting a nonlinear model of the agricultural machine into an approximate linear model by using a chain control theory, wherein when the agricultural machine moves along a curve anticlockwise, the control rate is as follows:
（3-2）
when the agricultural machinery moves clockwise along the curve, the control rate is as follows:
（3-3）
wherein,，a theoretical front wheel steering angle function related to transverse deviation and agricultural machine course deviation angle variables;
considering that the model accuracy of agricultural machinery will have a large influence on the control quality of agricultural machinery, in the formula (3-1), we consider that c (x) vssin θ dt is approximately 0, and ds is approximately dx, when the curvature of the tracking curve is small, the assumed condition can be basically satisfied, as the curvature increases, the accuracy of the agricultural machine model will decrease, the control performance will decrease, and the auxiliary control is performed after the desired steering compensation angle c is added, specifically, in step 3, after a theoretical front wheel steering angle (y, theta) is obtained by using a chain control theory, an expected steering compensation angle c is calculated by using an adaptive controller, the theoretical front wheel steering angle (y, theta) and the expected steering compensation angle c are added to obtain an actual steering angle e of the agricultural machine and output to an agricultural machine model, and the actual steering angle e of the agricultural machine is controlled to enable the agricultural machine to walk along a set curve;
the adaptive controller has 2 input variables, namely a transverse deviation variable y and a transverse deviation differential dy, the output of the adaptive controller is a desired steering compensation angle c of the agricultural machine, wherein the variable dy is taken as the trend of the transverse deviation of the agricultural machine, and the calculation formula of dy is as follows:
（4）
wherein, ytSelecting a parameter △ t as 1s, wherein the transverse deviation is the transverse deviation of the agricultural machinery at the moment t;
fuzzification of input and output variables
(1) Transverse deviation y
Basic domain of discourse: [ -60,60], quantization scale: { -6, -5, -4, -3, -2, -1,0,1,2,3,4,5,6}, with a quantization factor Ky =6/60= 0.1;
(2) lateral deviation differential dy
Basic domain of discourse: [ -6,6], quantization scale: { -6, -5, -4, -3, -2, -1,0,1,2,3,4,5,6}, with a quantization factor Ky =6/6= 1;
(3) compensation angle c
Basic domain of discourse: [ -8,8], quantization level: { -6, -5, -4, -3, -2, -1,0,1,2,3,4,5,6}, quantization factor Ky =6/8= 0.75;
the blur levels of the lateral deviation y, the lateral deviation differential dy and the compensation angle c 3 variables are: negative Big (NB), Negative Middle (NM), Negative Small (NS), Zero (ZO), Positive Small (PS), Positive Middle (PM) and Positive Big (PB), wherein the three membership functions all adopt Gaussian functions; the fuzzy control rules are shown in table 1:
table 1 fuzzy control rules.
For example, when the vehicle body is at the outer side of the curved path, the lateral deviation is the positive maximum and the lateral deviation tends to increase, the adaptive controller outputs the negative maximum expected steering compensation angle c, so that the lateral deviation of the agricultural machine is reduced, which is described by a fuzzy rule as: if y = PB and dy = PB, then c = NB; the other fuzzy rules are shown in table 1, there are 49 control rules in total, the car body is similar to the above example in other cases, and any case of the car body is not described any more.
The method is characterized by simulating the method by using matlab software, setting a path curve, setting the initial position of the agricultural machine as [0,0], setting the initial angle as 0rad, selecting Kd =0.6 and Kp =0.09, tracking the set curve by using the control method of the invention, wherein the abscissa of figures 8-11 is the driving distance of the agricultural machine, and the tracking curve is basically superposed with the set curve as can be seen from figure 8; as can be seen from fig. 9, the lateral deviation is maintained around 10 cm; as can be seen from fig. 10, the course deviation is about 0.02rad, wherein the point with large fluctuation is mainly that the slope of the curve is large, the sampling points are sparse, so that the M point is not very accurate, but in the actual operation process, the sampling points are dense, so that the problem of inaccuracy of the M point is solved; as can be seen from fig. 11, the steering angle of the front wheel is 1 order inertia link, and there is no sudden change, which is consistent with the reality; through the analysis, the control method disclosed by the invention is used for controlling the turning path of the agricultural machine, the control precision is high, and the agricultural machine basically walks according to the set curve path.
When the invention works, the visual machine camera collects the environmental information around the agricultural machine, whether the agricultural machine enters an obstacle avoidance system is confirmed according to the surrounding environmental information, if a small obstacle in front of the agricultural machine is detected, the agricultural machine enters an obstacle avoidance navigation state, the agricultural machine obtains the position information of the agricultural machine through the position sensor detection, the size of a characteristic circle of the obstacle in front of the agricultural machine and the distance between the agricultural machine and the obstacle are calculated, the size of the characteristic circle is determined according to the width of a plough of the agricultural machine and the minimum turning radius of the agricultural machine so as to set a safe distance, a theoretical obstacle avoidance path is set by using an improved shortest tangent method, but the control precision of the obstacle avoidance of the agricultural machine is reduced due to the discontinuous curvature of the theoretical obstacle avoidance path, the theoretical obstacle avoidance path is optimized by using a Bezier curve optimization method to generate a new actual obstacle avoidance path, the agricultural machine obtains the position information of the agricultural machine through the position sensor detection, the angle sensor detects the steering angle of a front wheel in real time and feeds, so as to more accurately control the front wheel turning angle of the agricultural machine, obtain the curve curvature, the course deviation and the transverse deviation of the set path in real time by using a curve tracking method, convert the original agricultural machine model into a linear model by using a chain control theory, control the chain system by using a state feedback control method, obtaining a theoretical front wheel steering angle through a state feedback controller, calculating an expected compensation steering angle through an adaptive controller, adding the theoretical front wheel steering angle and the expected compensation steering angle to obtain an actual front wheel steering angle, outputting the actual front wheel steering angle to an agricultural machinery model, detecting the position of an agricultural machinery in real time by a position sensor and sending position information to the state feedback controller and the adaptive controller, the agricultural machine is driven to travel along a set curve by controlling the steering angle of the front wheel of the agricultural machine, so that the agricultural machine automatically bypasses obstacles; according to the method, a theoretical obstacle avoidance path is calculated through an improved shortest tangent method, the theoretical obstacle avoidance path is optimized through a path optimization method based on a Bezier curve, the obstacle avoidance path is easier to control, in order to further improve the control accuracy of curve tracking, an expected steering compensation angle is added for auxiliary control, an agricultural machine is enabled to walk along a set curve, and the control accuracy is high; the device can be applied to the work of automatically avoiding small obstacles when the unmanned agricultural machine works in the field.
The present invention is not limited to the above embodiments, and based on the technical solutions disclosed in the present invention, those skilled in the art can make some substitutions and modifications to some technical features without creative efforts based on the disclosed technical solutions, and these substitutions and modifications are all within the protection scope of the present invention.