Research on the control strategy of sideslip and hysteresis of intelligent harvesting robot

Wang Zhuoxuan，Wang Lihui*，Xu Ninghui，Li Yong，Shi Jiachen

(1.School of Instrument Science and Engineering，Southeast University，Nanjing 210096，China;2.Key Laboratory of Micro-inertial Instrument and Advanced Navigation Technology，Ministry of Education，Nanjing 210096，China)

Abstract: Aiming at the characteristic that the heavy-load harvesters would easily slip and sideslip and its nonlinear largelag control system，a method of compensating the sideslip and relieving the hysteresis in path tracking for heavy-load harvesters was proposed in this paper.After establishing a kinematic model，the equivalent sideslipping angle was estimated according to the previous and the present state of the harvester，to compensate for the expected wheel steering angle.The featured state of the harvester can be predicted according to the present state and speed of the harvester to adjust the present lateral error and heading angle error to optimize the expected wheel steering angle calculated by the path tracking algorithm，which can improve the accuracy and stability when the heavy-load harvester is working.The validity of the proposed algorithm was verified by experiments at the speeds of 0.6 m/s and 1.0 m/s，respectively.The maximum lateral errors of the harvester were 8.3 cm and 13.9 cm，respectively，and the standard deviations were 3.01 cm and 4.09 cm respectively.The results show that the strategy proposed in this paper can effectively reduce the lateral error and achieve accurate path tracking control.

Keywords: intelligent heavy-load harvester;pure pursuit algorithm;nonlinear;control delay;sideslip

0 Introduction

Intelligent heavy-load harvesters can operate autonomously in the field and can effectively improve agricultural production efficiency and reduce work intensity of operators.The design and development of intelligent heavy-load harvesters is one of the current research hotspots in intelligent agriculture[1].The working environment of heavy-load harvesters is harsh;due to large volume and mass of the heavyload harvesters，the inertia and load of them are also large.Moreover，obvious hysteresis and nonlinearity of control systems and actuators have increased the difficulty of operation.At the same time，intelligent heavy-load harvesters are prone to sideslip in the field phenomenon，which affects the accuracy and stability of its automatic operation.Many scholars at home and abroad have studied the hysteresis and nonlinearity of the steering mechanism of heavy-load vehicles from various aspects.For example，Fang et al.[2]established a system model for a common hydraulic system，and designed a two-dimensional fuzzy controller to eliminate the interference caused by the nonlinearity，uncertainty，and multiple variables of the hydraulic system，and the simulation results show good robustness of the method.Kralev et al.[3]designed a highly robust controller for the electro-hydraulic steering system using a 32-bit microcontroller，and used the D-K iteration method to minimize the singular value of the closed-loop control system and obtained the optimal robustness.Other scholars applied the minimum value principle to optimize the control strategy[4];used the dual-channel fuzzy PID control strategy for optimization[5]，adopted the adaptive double closed-loop structure control strategy[6]，and achieved high robustness in design based on the Lyapunov method.The high robustness control of the hydraulic steering mechanism has been studied by using methods such as high-performance controllers[7].Some scholars have also studied the nonlinearity and hysteresis of the control system from the model level of the control system.For example，Li et al.[8]analyzed the application points and application scope of six typical nonlinear models，and expounded methods to improve the future development trend.Xie et al.[9]discussed a commonly used nonlinear lag discrete system，whose output finally met the requirements through successive iterations.Some scholars used the lead amount to compensate for the error caused by the lag[10]，optimized the search method of the preview point on the path[11]，integrated the dynamics and kinematics models to optimize the control parameters[12].The hysteresis and nonlinearity were studied from the system level by methods such as transforming the hysteresis system into a hysteresis-free system[13].At the same time，since vehicles are prone to sideslip，different scholars usedH∞control[14]，neural network[15-16]，sideslip compensation angle[17]，fuzzy control strategy[18]and feedback control[19]under the dynamic model to compensate for or suppress the interference caused by sideslip.Liu et al.[20]analyzed the research status and development trend of intelligent control technology in agriculture，and pointed out that intelligent control，decision-making，and collaboration are the keys to ensuring efficient，high-precision，and high-quality operations of intelligent agricultural machinery.

The existing research on the hysteresis and nonlinearity of the steering system and the system model requires a relatively accurate mathematical model of the system，and the model needs to be remodeled for different steering mechanisms，to solve the defects of poor universality，difficult parameter adjustment，cumbersome calculation，and high complexity，which are factors made it difficult and insufficient to implement the intelligent heavy-load harvester.However，the existing research on sideslip compensation is mostly based on the dynamic model with the disadvantages of a complex mathematical model，too many parameters and massive calculations.In contrast，the kinematic model has the advantages of a simple mathematical model and a small calculation amount in a low-speed environment.Given the above situation，in this paper，a strategy for sideslip compensation and hysteresis correction in the path tracking of an intelligent heavy-load harvester was proposed.When establishing the kinematic model of the harvesting robot，the equivalent sideslip angle is introduced to overcome the negative impact of the sideslip on path tracking and its value is estimated based on the pose information of the previous and current moments.Using the prediction idea，the pose information at the future time point can be predicted according to the pose information at the current moment，to correct the pose information at the current moment.The pure pursuit algorithm is used to calculate the optimized expected steering angle，to suppress the decline of path tracking stability caused by the lag of the system.

1 Kinematic modeling and sideslip control of harvesting robot

The intelligent heavy-load harvester is a kind of agricultural machinery with a large front axle load.Generally，the front wheel is the power wheel，and the rear wheel is the steering wheel.The vehicle body structure is supplemented by a hydraulic auxiliary steering system.Since the speed of the harvesting robot is low during automatic operation，the effect of lateral force can be ignored and the tire can be regarded as a rigid wheel，so a simplified twowheeled vehicle model was established[21-22].

As shown in Figure 1，the navigation coordinate systemX-O-Y，(x，y)is the front wheel coordinate point，m;vis the speed of the harvesting robot，m/s;Lwbis the wheelbase of the front and rear axles of the harvester，m;θhis the speed direction of the harvester，also known as the heading angle，rad;θwsis the steering angle of the rear wheel of the harvester，rad.The kinematic model of the harvesting robot can be obtained from the kinematic constraints and the relationship in the figure.

Figure 1 Simplified two-wheel model of the harvester

Figure 2 Sideslip angle

When modeling the kinematic model，the sideslip between the tire and the ground can be ignored.When the heavy-load harvester is operating in the actual field，it is easy to cause sideslip due to slippery mud and other reasons，so it is necessary to compensate for the interference caused by the sideslip.

As shown in Figure 2，the concept of equivalent sideslip angleθse[23]was introduced into the navigation coordinate systemX-O-Yto describe the sideslip degree of the harvesting robot and the unit is rad.

Figure 3 Schematic of the PP algorithm

However，the equivalent sideslip angle cannot be measured directly in most cases.The front-wheel sideslip angle and rear-wheel sideslip angle are defined below.

(1)Front-wheel sideslip angleθsfrefers to the angle between the actual speed directionθhand the direction of sideslipθs，rad.

(2)Rear wheel sideslip angle refers to the angle between the actual speed directionθhand the theoretical sideslip directionθh-theory，rad.Therefore，the following relationship can be obtained.

where，μsis the dimensionless sideslip correction coefficient.

The theoretical speed direction can be calculated according to the position and attitude information at the previous moment.Assuming that neither the speed nor the steering angle of the rear wheel changes during the control period，the theoretical speed direction is

where，θh-previs the actual speed direction at the previous moment，rad.

In the sideslip direction，the inertial measurement unit(IMU)was used to obtain the acceleration of the harvesting robot at the current time point，which can be obtained in combination with the pose information.

where，a x，a yare the acceleration values of the harvesting robot in the navigation coordinate system，m/s2.

The sideslip correction coefficient measures the severity of the sideslip.The theoretical position and pose information at the current moment can be estimated by the actual position and pose information at the previous moment，which is obtained by comparing it with the actual position and pose information at the current time point.If there is no change in speed and heading angle within the control cycleT，the theoretical position at the current time point is

where，xtheory，ytheoryare the theoretical positions at the current time point;xprev，yprevare the actual positions at the previous time point.The severity of the sideslip can be expressed as

The Real-time Kinematic-Global Navigation Satellite System (RTK-GNSS)was used to obtain position and pose information.Considering the influence of positioning error，the relationship betweenμsandddiffcan be expressed as equation(7).

where，dth1anddth2are the judgment threshold of sideslip severity，m.

According to the kinematic model of the harvester，the influence of the actual steering angle of the harvester and the effects of the equivalent sideslip angle，the front wheel sideslip angle，and the rear wheel sideslip angle on the steering radius satisfies the following relationship.

where，reis the steering radius affected by the equivalent sideslip angle;rris the steering radius affected by the rear wheel sideslip angle;andrfis the steering radius affected by the front wheel sideslip angle，and the unit of them is m.According to the kinematic model，the following equation can be obtained.

After Taylor expansion of equation(9)and ignoring the high-order term，that is，the relationship between the equivalent sideslip angle and the sideslip angle of the front and rear wheels is as equation(10).

2 Path tracking algorithm and hysteresis control of harvesting robot

The path tracking algorithm of the intelligent heavy-load harvester is based on the pure pursuit algorithm.It estimates the position of the future harvesting robot according to the speed and pose information at the current time point;corrects the lateral deviation and heading deviation at the current time point，calculates the expected steering angle with the corrected lateral deviation and heading deviation，to overcome the problem of accuracy degradation caused by the nonlinearity and hysteresis of the harvesting robot control system.

2.1 Pure pursuit algorithm

The pure pursuit algorithm is a geometric algorithm based on the vehicle kinematics model.The essence of the algorithm is to determine a preview point on the expected path，and then calculate the corresponding expected steering radius according to the deviation of the vehicle from the expected path[24-25].

As shown in Figure 3，the vehicle body coordinate systemX(b)-O(b)-Y(b)，Lldis the forward-looking distance，m;ris the turning radius，m;O′is the instantaneous turning center of the harvesting robot，and the point[x(b)，y(b)]is the coordinate of the preview point on the desired path under the current forwardlooking distance，m.According to Figure 3，the following relationship can be obtained.

Then the relationship between the turning radius and the foresight distance is

where，the value ofx(b)in the vehicle body coordinate system is[26]

where，dXTEis the lateral deviation of the harvesting robot and refers to the distance between the current position of the harvesting robot and the desired path，and its symbol is regarded as positive along the right side of the desired path，otherwise negative，with the unit of m;θeis the heading deviation of the harvesting robot，which refers to the angle between the current speed direction and the expected path direction of the harvesting robot.The counterclockwise rotation of it is considered as positive，vice versa as negative，and the unit is rad.

Combined with the kinematic model of the harvesting robot，the desired steering angle can be obtained as equation(14).

It can be seen from equation(14)that the pure pursuit algorithm uses the lateral error and heading error at the current moment to calculate the desired steering angle，without considering the hysteresis of the control system.

2.2 Hysteresis correction

Since the intelligent heavy-load harvester is a kind of equipment with a large front axle load，and its steering control system is mainly a hydraulic steering power system，while the hydraulic system itself is a high delay nonlinear system，and the robot has the characteristics of a large mass，large load and large inertia，which further increase the difficulty of control.The macro performance of the lag of the harvesting robot is that there is a certain time delay after the controller of the harvesting robot sends out the control signals to the execution of the structure.In this paper，a correction method based on the prediction idea was used to correct the pose information at the current time point by estimating the length of the delay time and predicting the pose information at the future time point.

As shown in Figure 4，in the navigation coordinate systemX-O-Y，the harvesting robot receives the pose information at point m and calculates the desired steering angle，and the command is executed only when it travels to pointn.Assume that the delay time of the harvesting robot from pointmto pointnastd，and it is generally composed of fixed time and dynamic time.The fixed time is mainly related to the algorithm complexity of the harvesting robot controller，the mechanical structure and steering control mechanism of the harvesting robot itself.

Figure 4 Schematic of predicting location

The dynamic time is generally related to the working environment of the harvesting robot.In the pure pursuit algorithm based on the kinematic model，the changes in the ground state and the load of the harvesting robot will eventually be reflected in the changes in the speed.The adaptive adjustment of the delay time according to the speed and acceleration can better improve the adaptivity of the harvesting robot.If the linear relationship is adopted，the relationship between the delay time and the speed and acceleration of the harvesting robot is

where，μvis the velocity correction coefficient，s2·m;μa is the acceleration correction coefficient，s3·m;tftis the fixed time，s.

After estimating the time and predicting the position ofnpoints，assuming that neither the speed nor the steering angle has changed during this period，then the change in the heading angle during this period is

where，l mnis the arc length of the harvesting robot running from pointmto pointn，m.

The distance between pointmand pointncan be expressed as

In the vehicle body coordinate system，the positional relationship between pointnand pointmcan be expressed as

Therefore，in the navigation coordinate system，the position relationship of pointnand pointmcan be expressed as

After predicting the positions of n points，calculate the new lateral deviation and heading deviation using n points and substitute them into equation(4)to optimize the expected steering angle calculated by the pure pursuit algorithm.Combined with equation(10)，the optimized expected steering angle and the compensated sideslip angle were input into the steering controller of the harvesting robot，which can overcome the problem of the sideslip of the intelligent heavy-load harvester during field operation and correct the lag and nonlinearity of the harvesting robot.

3 Experiment

To verify the effectiveness of the algorithm，a field experiment was carried out.The experimental platform was built based on an intelligent rice-wheat combined harvesting robot with a wheelbase of 3.75 m.The navigation and positioning components include a dual antenna RTK-GNSS system，a MEMS series IMU，a non-contact Hall sensor for the angle sensor，and a STM32 series microcontroller for the main control chip.

The experimental methods and conditions are as follows:the driver manually drives the harvester to harvest the wheat in the field and records the driving path as the reference path.The desired path is calculated from the reference path and the current position，that is，the current coordinate is used as the starting point of the desired path，and the subsequent coordinate points on the desired path are translated according to the current coordinate and the reference path.The experiments were carried out at the advancing speed of 0.6 m/s and 1.0 m/s in automatic driving.

The experimental parameters were set as follows:the satellite positioning and updating frequency was 5 Hz;the sampling frequency of IMU was 50 Hz;the control cycle is consistent with the frequency of satellite positioning，which is 5 Hz;considering the complexity of the algorithm，the performance of the processor and the hysteresis of the hydraulic steering system，the fixed delay time was 0.05 s.Considering the muddy road and the acceleration and deceleration performance of the harvesting robot，the acceleration correction coefficientμa was set as 0.1 s3·m and the speed correction coefficientμvwas set as 0.15 s2·m;The forward-looking distanceLldwas 4 m.Comprehensively considering the influence of satellite positioning error，the judgment thresholds of sideslip severitydth1，dth2were 0.02 m and 0.08 m respectively.The final relationship betweenμsandddiffis equation(20).

The installation error of RTK-GNSS and the installation error of IMU were corrected in the algorithm.The relationship between the lateral error of the experiment and the driving distance is shown in Figure 5.

Figure 5 Travel distance and lateral error

As can be seen from Figure 5，since the starting point of the desired path is the current coordinate，the lateral error starts from 0 cm.With the automatic operation of the harvesting robot，the lateral error oscillates around 0 cm.The oscillation of the lateral error is related to the sensitivity of the algorithm，the angle sensor，and the hydraulic system.The maximum and standard deviation of lateral error are shown in Table 1.

Table 1 Table of lateral error

The experimental results show that when the speed is 0.6 m/s，the maximum lateral error is 8.3 cm，and the standard deviation can reach 3.01 cm;when the speed is 1.0 m/s，the maximum lateral error is 13.9 cm，and the standard deviation can reach 4.09 cm，showing that the effect of path tracking was good and effective.

4 Conclusion

Aiming at the problem of sideslip and control hysteresis that reduce the accuracy and stability of path tracking when the intelligent heavy-load harvester is operating in the field，a pure pursuit strategy of the intelligent heavy-load harvester was proposed to compensate for the sideslip and correct the lag.In the kinematic modeling，the concept of sideslip angle was introduced，and the controller estimated the theoretical position of the current moment according to the actual position of the harvesting robot at the previous moment.Combined with the actual position at the current moment and the equivalent sideslip angle estimated by IMU，the expected steering angle calculated by the pure pursuit algorithm was compensated to overcome the decline in tracking accuracy caused by the sideslip.Aiming at the nonlinearity and lag of the control system，the controller estimated the pose information of the future time according to the speed and pose information of the harvesting robot at the current moment，corrected the pose information at the current moment，calculated the new lateral error and heading error，and optimized the expected steering angle calculated by the pure pursuit algorithm.The optimized desired steering angle and the compensated sideslip angle were input into the steering controller at the same time to alleviate the error caused by the hysteresis and sideslip of the control system of the intelligent heavy-load harvesting robot.The effectiveness of the algorithm was verified by experiments at the speed of 0.6 m/s and 1.0 m/s.The results show that the maximum lateral error is 8.3 cm and 13.9 cm，and the standard deviation is 3.01 cm and 4.09 cm respectively，demonstrating that effect of path tracking is satisfying and effective.