CN113359710B - LOS theory-based agricultural machinery path tracking method - Google Patents

Abstract

The invention relates to an agricultural machinery path tracking method based on LOS theory, which belongs to the field of agricultural machinery intelligent control, wherein when an agricultural machinery runs on a set straight path, a navigation positioning module acquires real-time position information of the agricultural machinery; transmitting the position information to an LOS algorithm device through parameters such as coordinates and transverse tracking errors analyzed by an upper computer system to obtain a current driving course angle; and finally, the front wheel feedback controller outputs the wheel steering angle expected by the system according to the course angle and the transverse tracking error, so that the steering deviation angle of the wheels can be obtained, and the steps are repeated to realize path tracking control. The agricultural machinery system based on the LOS navigation method can be suitable for different agricultural machinery operation scenes, effectively solves the problem of overlarge path tracking error, and has the characteristics of robustness, accuracy and the like. The method has clear and simple thought, and can enable the change of the wheel turning angle of the agricultural machinery to be more smooth and stable in the process of tracking the expected path under the condition of external interference or uncertain system model parameters.

Description

A Path Tracking Method of Agricultural Machinery Based on LOS Theory

technical field

The invention relates to an agricultural machinery path tracking method based on LOS theory, and belongs to the technical field of agricultural machinery intelligent control.

Background technique

my country is a large agricultural country. Agriculture is the foundation of social and economic development and the guarantee of people's material life. In order to promote the improvement of agricultural productivity and realize the development of agricultural modernization, people put forward the concept of precision agriculture. It mainly uses navigation satellite positioning technology , sensor technology, remote sensing control and other technologies to complete the autonomous operation of agricultural machinery, the purpose of automatic navigation, effectively reducing the time of farmland cultivation, improving the efficiency of farming, and can replace human labor to achieve automatic driving, relieve the driver's work fatigue and reduce production. Accident rate, improve production safety.

Among them, the path tracking method of agricultural machinery, as the key technology of automatic navigation of agricultural machinery, has always been a research hotspot in the field of intelligent control technology of agricultural machinery. The existing agricultural machinery path tracking technology mainly relies on the navigation and positioning technology of the upper computer system of agricultural machinery, the sensor accuracy of the lower computer system, and hydraulic steering to control, which greatly increases the complexity of system design and the dependence of control models, increases production costs, and reduces The production efficiency is reduced, and the adaptability of the agricultural machinery operation environment is not strong. Therefore, on the premise of simplifying the structure of the automatic navigation system of agricultural machinery, how to optimize a set of path tracking controller with strong robustness and high accuracy is the key to realize the precision and intelligence of agriculture.

Contents of the invention

The invention provides a path tracking control method for agricultural machinery based on the LOS theory, which can stably and effectively perform path tracking control on unmanned agricultural machinery under the condition of uncertain system model parameters or large disturbances in the operating environment, so that the agricultural machinery can be controlled in the original hardware system structure. The simplified basis ensures the stability and accuracy of the nonlinear feedback controller.

In order to solve the above technical problems, the present invention takes the following technical solutions:

A method for tracking the path of agricultural machinery based on LOS theory, comprising the following steps:

S1, when the agricultural machinery is driving on the set straight path, the real-time location information of the agricultural machinery is obtained by the navigation and positioning module;

S2, the position information is transmitted to the LOS algorithm through the coordinates analyzed by the upper computer system, the lateral tracking error and other parameters to obtain the current heading angle;

S3, the front wheel feedback controller outputs the wheel steering angle expected by the system according to the heading angle and lateral tracking error, and thus the steering deviation angle of the wheel can be obtained, and the above steps are repeated to realize path tracking control.

Further, the step S1 specifically includes:

S11, set the boundary of the agricultural machinery working field through the Beidou RTK positioning module and the visual recognition CDD module, and then draw several parallel straight-line trajectories.

S12, the Beidou RTK navigation module outputs the position and track point information of the agricultural machinery in real time, and at the same time transforms the coordinates of the three vertices of the rectangular field into the Gaussian plane coordinate system, and stores the planned path in the navigation system in the form of a two-dimensional array. Realize navigation path planning.

Further, the step S11 is specifically:

Use the RTK-CCD positioning vision sensor fusion system to measure the three coordinate vertices of the test field to determine a rectangle, which is assumed to be the positioning coordinates of the headland nodes A, B, C, and D; the principle of the longest distance between each operation line, combined with the actual geometry of the plot Shape, select the boundary AD as the baseline for the planning of the operation line, and draw several parallel operation lines based on it. The distance between the parallel lines depends on the distance between the operation lines. The shortest distance between the last operation line and the boundary BC should not be less than 1/2 A working distance, all Q parallel working lines and the focal points of boundaries AB and CD are used as planned working line nodes, and the corresponding working line nodes are stored in a two-dimensional array for calling by the navigation control software based on the LOS algorithm.

Further, the step S2 specifically includes:

和

定义的直线跟踪路径，并且路径坐标系的原点在

农机在路径固定的坐标系内坐标为pⁿ(t)＝[x(t),y(t)]，且满足公式：

S21: Determine a path consisting of two path points

and

The defined line traces the path, and the origin of the path coordinate system is at

The coordinates of the agricultural machinery in the fixed path coordinate system are p ⁿ (t)=[x(t),y(t)], and satisfy the formula:

In the formula:

α_k＝atan2(y_k+1-y_k,x_k+1-x_k)∈S；

α _k ＝atan2(y _k+1 -y _k , x _k+1 -x _k )∈S;

α _k is the angle between the true north of the geodetic coordinate system and the expected path P _k and P _k+1 , and a is the coefficient;

S22: Let ε(t)=[s(t),e(t)] ^T ∈ R ² , where:

s(t)＝[x(t)-x _k ]cos(α _k )+[y(t)-y _k ]sin(α _k )

e(t)＝-[x(t)-x _k ]sin(α _k )+[y(t)-y _k ]cos(α _k )

s(t) is the path tracking distance of the agricultural machinery, and e(t) is the lateral tracking error. In the actual path tracking process, we only need to pay attention to the lateral deviation of the agricultural machine itself, because when the lateral deviation e(t)=0, it means The agricultural machinery has converged on the expected tracking path, and the path tracking control target is:

S23: Set the LOS navigation algorithm as:

In the formula:

为速度—路径相关角，其中Δ为农机的前视距离，e为横向跟踪误差；

is the speed-path correlation angle, where Δ is the look-ahead distance of the agricultural machinery, and e is the lateral tracking error;

则航向角为：If the driving direction angle of the agricultural machinery is

Then the heading angle is:

Further, the step S3 specifically includes:

S31: Obtain information such as the real-time position, wheel speed, and rotation angle of the agricultural machinery through the navigation and positioning system and the pose acquisition system, and carry out kinematic modeling in combination with the structural parameters of the agricultural machinery and the two-degree-of-freedom model theory, specifically:

Considering the path tracking process of agricultural machinery as a low-speed movement in the X-Y plane coordinate system, according to the upper and lower triangular chord theorem:

Where δ _f , δ _r are the front and rear wheel angles respectively; l _f , l _r are the front wheelbase and rear wheelbase respectively; β is the speed direction angle of the agricultural machinery; R is the turning radius;

得：Simplify the two equations and multiply both sides by

have to:

Under the conditions of low-speed driving of agricultural machinery, the vehicle direction change rate is:

即

which is

Since the agricultural machinery does not consider the steering of the rear wheels, that is, when tan(δ _r )=0, we can get:

为航向角，v为行驶速度，δ为前轮转角；in

is the heading angle, v is the driving speed, and δ is the front wheel rotation angle;

In a low-speed operating environment, assuming that the direction of the wheel rotation angle is consistent with the direction of the speed, that is, δ=β, the kinematic model is:

S32: From the steering characteristics of agricultural machinery, the front wheel angle can be obtained:

为航向角，可由LOS算法器求出；δ_e(t)为转向偏差角；in

is the heading angle, which can be obtained by the LOS algorithm; δ _e (t) is the steering deviation angle;

Without considering the driving tracking deviation, the larger the lateral tracking error e(t), the larger the front wheel steering angle, assuming that the expected trajectory of the vehicle intersects the nearest tangent line on the given path at a distance d(t) from the front wheel extension line, According to the geometric relationship, the following nonlinear proportional function is obtained:

Among them, d(t) is related to vehicle speed, expressed by vehicle speed v(t) and gain parameter k.

Without considering the lateral tracking error, the steering deviation angle of the front wheels is consistent with the tangent direction of the expected path, that is, when there is no lateral error, the direction of the front wheels is the same as the expected path direction:

As the lateral tracking error increases, the non-linear scaling function produces a slip angle that points directly to the desired path, and:

In order to make the two differential equations have a global asymptotically stable balance at the junction of the zero point error, the nonlinear front wheel feedback controller is designed as follows:

And according to the geometric relationship:

so

Therefore, the convergence speed of e(t) is between the linear convergence speed of v(t) and the exponential convergence speed of the gain parameter k, when the lateral tracking error e(t) is small, (ke(t)/v(t) ) ² tends to 0, then

积分可得：

Points can be earned:

e(t)=e(0)*exp ^-kt

For any lateral error, the differential equation converges monotonously to 0, and the path tracking control goal is realized.

Further, the wheel steering deviation information obtained by the navigation controller is transmitted to the lower computer system of agricultural machinery, the real deviation value is corrected by the value of the steering sensor and the photoelectric encoder, and it is transmitted to the hydraulic system of the control mechanism to control the hydraulic system The flow and direction of the flow, and finally control the steering of the wheels, so that the vehicle can travel according to the set route.

Through the above technical solutions, compared with the prior art, the present invention has the following beneficial effects:

In order to solve the problems of poor adaptability, complex system design and unstable control of agricultural machinery path tracking technology, the invention discloses an agricultural machinery path tracking method based on LOS theory; the LOS algorithm is used to obtain the heading angle of agricultural machinery path tracking, and it can automatically switch tracking The path target point ensures the update of state parameters such as the lateral tracking error input by the current controller, realizes real-time control of steering, and can converge to the desired tracking path more quickly and stably; and the LOS algorithm does not depend on the system parameter model When it is large, it can also output accurate heading angle of agricultural machinery. Combined with the designed front wheel feedback nonlinear controller, the adaptability of agricultural machinery operation environment is enhanced, and the overall control system framework is simplified at the same time. It has high robustness and is suitable for agricultural machinery.

Description of drawings

Figure 1 is a structural diagram of the path tracking system for agricultural machinery based on the LOS theory;

Figure 2 is a schematic diagram of the LOS control algorithm;

Fig. 3 is the agricultural machinery kinematics model diagram;

Figure 4 is a simulation comparison diagram of two system control methods; (a) is a comparison diagram of expected tracking and actual tracking paths; (b) is a comparison diagram of expected tracking and tracking paths based on the LOS method.

Detailed ways

In order to further understand the features, technical means, and specific objectives and functions achieved by the present invention, the present invention will be further described in detail below in conjunction with the accompanying drawings and specific embodiments.

As shown in the system structure diagram in Figure 1, a method for tracking the path of agricultural machinery based on LOS theory includes the following steps:

S1, when the agricultural machinery is driving on the set straight path, the real-time location information of the agricultural machinery is obtained by the navigation and positioning module;

S2, the position information is transmitted to the LOS algorithm through the coordinates analyzed by the upper computer system, the lateral tracking error and other parameters to obtain the current heading angle;

S3, the front wheel feedback controller outputs the wheel steering angle expected by the system according to the heading angle and lateral tracking error, and thus the steering deviation angle of the wheel can be obtained, and the above steps are repeated to realize path tracking control.

The step S1 specifically includes:

S11, set the boundary of the agricultural machinery working field through the Beidou RTK positioning module and the visual recognition CDD module, and then draw several parallel straight-line trajectories.

S12, the Beidou RTK navigation module outputs the position and track point information of the agricultural machinery in real time, and at the same time transforms the coordinates of the three vertices of the rectangular field into the Gaussian plane coordinate system, and stores the planned path in the navigation system in the form of a two-dimensional array. Realize navigation path planning.

The step S11 is specifically:

Use the RTK-CCD positioning vision sensor fusion system to measure the three coordinate vertices of the test field to determine a rectangle, which is assumed to be the positioning coordinates of the headland nodes A, B, C, and D; the principle of the longest distance between each operation line, combined with the actual geometry of the plot Shape, select the boundary AD as the baseline for the planning of the operation line, and draw several parallel operation lines based on it. The distance between the parallel lines depends on the distance between the operation lines. The shortest distance between the last operation line and the boundary BC should not be less than 1/2 A working distance, all Q parallel working lines and the focal points of boundaries AB and CD are used as planned working line nodes, and the corresponding working line nodes are stored in a two-dimensional array for calling by the navigation control software based on the LOS algorithm.

As shown in Figure 2, based on the LOS navigation algorithm processing process, step S2 specifically includes:

和

定义的直线跟踪路径，并且路径坐标系的原点在

农机在路径固定的坐标系内坐标为pⁿ(t)＝[x(t),y(t)]，且满足公式：

S21: Determine a path consisting of two path points

and

The defined line traces the path, and the origin of the path coordinate system is at

The coordinates of the agricultural machinery in the fixed path coordinate system are p ⁿ (t)=[x(t),y(t)], and satisfy the formula:

In the formula:

α_k＝atan2(y_k+1-y_k,x_k+1-x_k)∈S；

α _k ＝atan2(y _k+1 -y _k , x _k+1 -x _k )∈S;

α _k is the angle between the true north of the geodetic coordinate system and the expected path P _k and P _k+1 .

S22: Let ε(t)=[s(t),e(t)] ^T ∈ R ² , where:

s(t)＝[x(t)-x _k ]cos(α _k )+[y(t)-y _k ]sin(α _k )

e(t)＝-[x(t)-x _k ]sin(α _k )+[y(t)-y _k ]cos(α _k )

s(t) is the path tracking distance of agricultural machinery, and e(t) is the lateral tracking error. In the actual path tracking process, we only need to pay attention to the lateral deviation of the agricultural machine itself, because when the lateral deviation e(t)=0, it means The agricultural machinery has converged on the expected tracking path, and the path tracking control target is:

S23: Set the LOS navigation algorithm as:

In the formula:

为速度—路径相关角。其中Δ为农机的前视距离，e为横向跟踪误差；

is the velocity-path relative angle. Where Δ is the forward-looking distance of the agricultural machinery, and e is the lateral tracking error;

则航向角为：If the driving direction angle of the agricultural machinery is

Then the heading angle is:

Described step S3 specifically comprises:

S31: As shown in Figure 3, obtain information such as the real-time position, wheel speed, and rotation angle of the agricultural machinery through the navigation and positioning system and the pose acquisition system, and carry out kinematic modeling in combination with the structural parameters of the agricultural machinery and the two-degree-of-freedom model theory, specifically:

Considering the path tracking process of agricultural machinery as a low-speed movement in the X-Y plane coordinate system, according to the upper and lower triangular chord theorem:

Among them, δ _f and δ _r are the front and rear wheel rotation angles respectively; l _f and l _r are the front wheelbase and rear wheelbase respectively;

β is the direction angle of the agricultural machinery driving speed; R is the turning radius;

得：Simplify the two equations and multiply both sides by

have to:

Under the conditions of low-speed driving of agricultural machinery, the vehicle direction change rate is:

即

which is

Since the agricultural machinery does not consider the steering of the rear wheels, that is, when tan(δ _r )=0, we can get:

为航向角，v为行驶速度，δ为前轮转角。in

is the heading angle, v is the driving speed, and δ is the front wheel rotation angle.

In a low-speed operating environment, assuming that the direction of the wheel rotation angle is consistent with the direction of the speed, that is, δ=β, the kinematic model is:

S32: From the steering characteristics of agricultural machinery, the front wheel angle can be obtained:

为航向角，可由LOS算法器求出；δ_e(t)为转向偏差角。in

is the heading angle, which can be obtained by the LOS algorithm; δ _e (t) is the steering deviation angle.

Without considering the driving tracking deviation, the larger the lateral tracking error e(t), the larger the front wheel steering angle, assuming that the expected trajectory of the vehicle intersects the nearest tangent line on the given path at a distance d(t) from the front wheel extension line, According to the geometric relationship, the following nonlinear proportional function is obtained:

Among them, d(t) is related to vehicle speed, expressed by vehicle speed v(t) and gain parameter k.

Without considering the lateral tracking error, the steering deviation angle of the front wheels is consistent with the tangent direction of the expected path, that is, when there is no lateral error, the direction of the front wheels is the same as the expected path direction:

As the lateral tracking error increases, the non-linear scaling function produces a slip angle that points directly to the desired path, and:

In order to make the two differential equations have a global asymptotically stable balance at the junction of the zero point error, the nonlinear front wheel feedback controller is designed as follows:

And according to the geometric relationship:

so

Therefore, the convergence speed of e(t) is between the linear convergence speed of v(t) and the exponential convergence speed of the gain parameter k, when the lateral tracking error e(t) is small, (ke(t)/v(t) ) ² tends to 0, then

积分可得：

Points can be earned:

e(t)=e(0)*exp ^-kt

For any lateral error, the differential equation converges monotonously to 0, and the path tracking control goal is realized.

Through the agricultural machinery upper computer and lower computer and other auxiliary equipment such as GPS, position and attitude sensors and angle sensors, the information such as position and angle will be collected, and the information will be fed back between the controller and the hydraulic steering transmission system.

As shown in Figure 4, through the simulation comparison of path tracking control, it can be concluded that the agricultural machinery path tracking control method based on the LOS theory of the present invention can make the control target more quickly and stably converge to the desired path, and the path tracking During the process, the number of corner changes is less, the process is more gentle, and has a good control effect.

The above descriptions are only preferred embodiments of the present invention, and are not intended to limit the present invention. Any modifications, replacements and improvements, as long as they do not deviate from the principles and rules of the present invention, are within the protection scope of the present invention.

Claims (4)

1. An agricultural machinery path tracking method based on an LOS theory is characterized by comprising the following steps:

s1, when an agricultural machine runs on a set straight path, a navigation positioning module acquires real-time position information of the agricultural machine;

s2, transmitting the position information to an LOS algorithm through coordinates and transverse tracking error parameters analyzed by an upper computer system to obtain a current driving course angle;

the step S2 specifically includes:

s21: determining a two-way path point

And

the defined straight line traces the path, and the origin of the path coordinate system is at

The coordinate of the agricultural machine in the coordinate system with the fixed path is p ⁿ (t)＝[x(t),y(t)]And satisfies the formula:

in the formula:

α _k ＝a tan2(y _k+1 -y _k ,x _k+1 -x _k )∈S；

α _k for the north and the north of the geodetic coordinate system and the expected path P _k And P _k+1 A is a coefficient;

s22: let ε (t) = [ s (t), e (t) ]] ^T ∈R ² Wherein:

s(t)＝[x(t)-x _k ]cos(α _k )+[y(t)-y _k ]sin(α _k )

e(t)＝-[x(t)-x _k ]sin(α _k )+[y(t)-y _k ]cos(α _k )

s (t) is the tracking distance of the agricultural machinery path, e (t) is the transverse tracking error, and in the actual path tracking process, only the transverse deviation of the agricultural machinery running is needed to be concerned, because when the transverse deviation e (t) =0, the agricultural machinery is converged on the expected tracking path, the path tracking control target is as follows:

s23: the LOS navigation algorithm is set as follows:

in the formula:

is the speed-path correlation angle, where Δ is the forward-looking distance of the agricultural machine and e is the lateral tracking error;

if the agricultural machine is used as the driving direction angle

The course angle is then:

s3, the front wheel feedback controller outputs the wheel steering angle expected by the system according to the course angle and the transverse tracking error, so that the steering deviation angle of the wheels can be obtained, and the steps are repeated to realize path tracking control;

the step S3 specifically includes:

s31: the method comprises the steps of acquiring real-time position, wheel speed and corner information of the agricultural machine through a navigation positioning system and a pose acquisition system respectively, and performing kinematic modeling by combining structural parameters of the agricultural machine and a two-degree-of-freedom model theory, wherein the method specifically comprises the following steps:

considering the path tracking process of the agricultural machinery as low-speed motion in an X-Y plane coordinate system, the following results are obtained according to the upper triangular chord and the lower triangular chord theorem:

wherein delta _f 、δ _r Respectively a front wheel corner and a rear wheel corner; l. the _f 、l _r Respectively a front wheel base and a rear wheel base; beta is the direction angle of the running speed of the agricultural machine; r is a turning radius;

two-way degeneracy is multiplied on both sides

Obtaining:

under the condition of low-speed running working condition, the direction change rate of the agricultural machine is as follows:

namely that

Since the agricultural machinery does not take into account the rear wheel steering situation, i.e. tan (delta) _r ) When the color is =0, finishing:

wherein

Is a course angle, v is a running speed, and delta is a front wheel rotation angle;

under a low-speed working environment, assuming that a wheel rotation angle direction is consistent with a speed direction, namely δ = β, a kinematic model is as follows:

s32: the front wheel steering angle can be obtained by the steering characteristic of the agricultural machinery:

wherein

The course angle can be obtained by an LOS algorithm; delta _e (t) is the steering deviation angle;

the larger the lateral tracking error e (t) and the larger the front wheel steering angle without considering the driving tracking deviation, the following nonlinear proportional function is obtained according to the geometrical relationship, assuming that the expected trajectory of the vehicle intersects the nearest tangent on a given path from the front wheel extension line d (t):

wherein d (t) is related to vehicle speed, represented by vehicle speed v (t) and gain parameter k;

the front wheel steering deviation angle coincides with the desired path tangent direction without considering the lateral tracking error, i.e. the front wheel direction is the same as the desired path direction in the absence of lateral error:

as the lateral tracking error increases, the non-linear proportional function produces a front wheel slip angle that points directly to the desired path, and:

in order to enable the two differential equations to have global gradual stable balance at the zero point error junction, the nonlinear front wheel feedback controller is designed by integrating two control factors as follows:

and from the geometrical relationship:

therefore, it is possible to

Therefore, the convergence rate of e (t) is between the linear convergence rate of v (t) and the exponential convergence rate of gain parameter k, and when the lateral tracking error e (t) is small, (ke (t)/v (t)) ² Approaching 0, then

Integration can give:

e(t)＝e(0)*exp ^-kt

for any transverse error, the differential equation is monotonously converged to 0, and the path tracking control target is realized.

2. The LOS theory-based agricultural machinery path tracking method according to claim 1, wherein the step S1 specifically comprises:

s11, setting the boundary of the agricultural machine working field block through a Beidou RTK positioning module and a visual recognition CDD module, and then dotting a plurality of parallel linear tracks;

and S12, the Beidou RTK navigation module outputs the position and track point information of the agricultural machine in real time, simultaneously converts the position and track point information of the agricultural machine and three vertex coordinates of the rectangular field block into a Gaussian plane coordinate system, and stores the planned path into a navigation system in a two-dimensional array form to realize navigation path planning.

3. The method for tracking the agricultural machinery path based on the LOS theory as claimed in claim 2, wherein the step S11 is specifically as follows:

determining three coordinate vertexes of a test field by using an RTK-CCD positioning vision sensor fusion system to determine a rectangle, and assuming positioning coordinates of ground nodes A, B, C and D; taking the longest distance of each operation line as a principle, combining the actual geometric shape of the plot, selecting a boundary AD as a reference line of operation line planning, using the boundary AD as the reference line to define a plurality of parallel operation lines, wherein the distance of the parallel lines is determined according to the operation line spacing, the shortest distance between the last operation line and the boundary BC is not less than 1/2 of the operation spacing, using the focuses of all Q parallel operation lines and the boundaries AB and CD as planning operation line nodes, and storing the corresponding operation line nodes into a two-dimensional array for being called by navigation control software based on an LOS algorithm.

4. The agricultural machinery path tracking method based on the LOS theory as claimed in claim 1, wherein the steering deviation information of the wheels obtained by the navigation controller is transmitted to an agricultural machinery lower computer system, the real deviation value is corrected by the values of the steering sensor and the photoelectric encoder, and the corrected deviation value is transmitted to a hydraulic system of a control mechanism to control the flow and the flow direction of the hydraulic system, and finally the steering of the wheels is controlled, so that the vehicle can run according to a set route.

Images