Stability enhancement method and experiment of orchard vehicle control

1. Introduction

Orchard vehicle, as a typical agricultural and versatile machine, has a lot of different tasks to carry out, such as seeding, spraying, fertilization, pruning and harvesting, etc. As the orchard vehicle often worked on sloping or uneven ground, and the center of mass is raised by lift platform, orchard vehicle rollovers cause over 45 % of fatalities in agricultural accidents each year [1]. The stability loss on rough ground is mainly because wheels follow bumps and hollows of rough ground and cause the instable of the vehicle [2]. So, it is very useful to design a control method which is able to keep orchard vehicle stable in various situations.

Nowadays, rollover protective structure (ROPS) is very popular in protecting the driver from rollover accidents [3-5]. Thambiratnam [6] researched on the performance of bulldozer rollover protective structure, and did a lot of work to solve the contradictory between ROPS bearing requirements and energy absorption requirements. Clark [7] extracted ROPS energy absorbing components for dynamic simulation analysis; he installed a thin cone tube as the energy absorption component on every side of ROPS to improve the energy absorption capacity. In order to study the behaviour of the ROPS mechanism in dynamic and static loading, Clark [8] also use finite element technology to simulate the nonlinear response of agricultural vehicles ROPS which is under the lateral concentrated static load.

But orchard vehicle has to be move freely between canopies of trees; it is impractical to be equipped with a ROPS. To provide necessary protection to drivers working in such a confined and complex environment, researchers should develop an active safety control method [9-12].

The goal of this paper is to design an active safety control method for typical orchard vehicle, which can suitable for its rough and changeable working environment. The experiment of control system, sloped surface and obstacles are carried out and the feasibility of control algorithmic verified.

2. Vehicle model

Ignoring the influence of non-sprung mass and lateral wind, considering the nonlinear factors such as front steering angle and the coupling of vertical movement and lateral movement when braking [13-16], 5 degree of freedom orchard vehicle rollover dynamics model has been set up which is shown in Fig. 1.

The dynamics differential equations ca n be got from Fig. 1 as following:

Longitudinal motion equation:

Lateral movement equation:

Yaw motion equation:

Roll motion equation:

Pitch motion equation:

where, $m$ is mass of orchard vehicle, $m_s$ is vehicle sprung mass, $a_x$, $a_y$ are vehicle acceleration along the direction of the $x$ and $y$, $r$ is yaw velocity, ${\phi }_y$ is roll angle, ${\phi }_x$ is pitch angle, $a$, $b$ are respectively distance from center of mass to epipodium and back shaft, $I_x$, $I_z$ are vehicle inertia around $X$ axis and $Z$ axis, $\delta$ is steering angle of front wheel; $h$, $h_{cm}$ are vehicle roll length and height of the center of gravity, $k_{\phi y}$ is suspension equivalent roll stiffness, $k_{\phi x}$ is suspension equivalent pitch stiffness, $c_{\phi y}$ is suspension equivalent roll damping, $c_{\phi x}$ is suspension equivalent pitch damping, $l_w$ is vehicle width, $f_{xi}$, $f_{yi}$ ($i=$1,…, 4) is braking force and lateral force for each tire.

Fig. 1Five-DOF model of orchard vehicle rollover and tip-over

3. Orchard vehicle bodywork posture control algorithm

Three axis accelerometer and gyro scope module of active safety control system (will describe in following section), which is installed in the orchard vehicle centroid, is used to obtain current $X$ axis angle ${\theta }_x$ and angular velocity ${\theta }_x^{\text{'}}$, $Y$ axis angle ${\theta }_y$ and angular velocity ${\theta }_y^{\text{'}}$.

By processing and analysing points sampling data, orchard vehicle ratio coefficient matrix of $X$ axis and $Y$ axis, $K_x=[k_{x1},k_{x2}]$ and $K_y=[k_{y1},k_{y2}]$, can be determined. $k_{x1}$ is restoring force coefficient of $X$ axis direction and $k_{x2}$ is damping coefficient of $X$ axis direction. $k_{y1}$ is restoring force coefficient of the $Y$ axis direction and $k_{y2}$ is damping coefficient of $Y$ axis direction.

After that, obtain $(k-1)$-point estimating value matrixes of angle and angular velocity of $X$ and $Y$ axes $X_{(k-1)x}=[{\theta }_{\left(k-1\right)x},{\theta }_{\left(k-1\right)x}^{\text{'}}]$ and $X_{(k-1)y}=[{\theta }_{\left(k-1\right)y},{\theta }_{\left(k-1\right)y}^{\text{'}}]$ with $\mathrm{k}$points sampling data. ${\theta }_{(k-1)x}$ is estimated value of angle $X$ axis at sample time $k-1$, ${\theta }_{\left(k-1\right)y}^{\text{'}}$ is estimated value of the angular velocity of $X$ axis of $k-1$ points, ${\theta }_{(k-1)y}$ is $(k-1)$-point estimated value of angle $Y$ axis, ${\theta }_{\left(k-1\right)y}^{\text{'}}$ is $(k-1)$-point estimated value of the angular velocity of $Y$ axis.

Then, using apriori estimate covariance matrix on $X$ axis and $Y$ axis $Q_{(k-1)x}$ and $Q_{(k-1)y}$, and measurement variance matrix of orchard vehicle posture control system $P_{zx}$, $P_{zy}$ to calculate $k$-point estimation gain matrix $k_{gx}$ and $k_{gy}$:

where, $k_{g\theta kx}$ is $k$-point estimates of gain of $X$ axis angle, $k_{g\theta \text{'}kx}$ is $k$-point estimates of gain of $X$ axis angle angular velocity, $I=\left[\mathrm{1,0}\right]$, $k_{g\theta ky}$, $k_{g\theta kx}$ is $k$-point estimates of gain of $Y$ axis angle, $k_{g\theta \text{'}ky}$ is $k$-point estimates of gain of $Y$ axis angle angular velocity.

Obtain measurement results matrix $Z_{kx}=[Z_{k\theta x},Z_{k\theta \text{'}x}]\text{,}$$Z_{ky}=[Z_{k\theta y},Z_{k\theta \text{'}y}]$ with the measurement results of orchard vehicle posture control system. $Z_{k\theta x}$ is $k$-point dip direction of $X$ axis, $Z_{k\theta \text{'}x}$ is $k$-point angular velocity of $X$ axis, $Z_{k\theta y}$ is $k$-point dip direction of $Y$ axis, $Z_{k\theta \text{'}y}$ is $k$-point angular velocity of $Y$ axis.

Solve $\mathrm{k}$-point estimated value matrix of angle and angular velocity $X_{kx}$ and $X_{ky}$:

Calculate apriori estimate covariance matrix at $k$-point $Q_{kx}$ and $Q_{ky}$ to facilitate the subsequent iteration:

At last, get $k$-point orchard vehicle output acceleration control results $\mathrm{a}$and angle of rotation $\varphi$in the $\mathrm{k}$moment. The flow chart of control algorithm is shown in Fig. 2:

4. Experiment results and analysis

A full-sized vehicle is difficult to operate and involves significant safety issues in unstable driven application which involves vehicle rollover and tip-over. It is more convenient to use a 1:8 scaled vehicle to test active safety control algorithm, which is mentioned in previous section. The developed control algorithm will be validated with the scaled vehicle since lateral and vertical dynamics of it are similar to those of a full-sized vehicle.

Fig. 2Flow chart of bodywork posture control algorithm

Fig. 3Main structure of experimental system

A microcontroller (STM32F103ZET6) from STM microelectronics is used for real-time data acquisition and control of the scaled vehicle speed and steering. The microcontroller is installed on the test vehicle. It samples the data from sensors and wirelessly sends them to a ZigBee receiver connected to a computer. MPU6050 from STMicroelectronics, which combines a 3-axis gyroscope, 3-axis accelerometer and a Digital Motion ProcessorTM (DMP), are used to measure acceleration and angle rate, respectively. It is placed near the C.G. of the vehicle, to measure the yaw, roll, pitch angle and rate. The front wheel of vehicle is driven by DC geared motor (SK3530). Microcontroller send control signal to drive motor control module (TB6612FNG). The speed of front wheel (which include right and left side) is measured by E6A2-CS3C speed encoder from OMRON. The photographs of the microcontroller and the sensors are shown in Fig. 3.

Fig. 4Scaled test path

a) First experiment

b) Second experiment

For the purpose of verifying and revising control model, we programmed the microcontroller to set a series of fixed speed and steering inputs. Thus, identical experiment can be repeated many times. For the first experiment, one obstacle is put $l_r=$ 2.5 m in the front of starting point, the route of this experiment is show in Fig. 4. The pitch angle of obstacle is $\theta =$22.5°. In this case, vehicle comes close to tip-over. The scaled vehicle differential speed curve, which is controlled by active safety control algorithm (ASC), is shown in Fig. 5.

Fig. 5Scaled vehicle differential speed curve and 2-axis angle with ASC

a) The speed of each wheel

b) Pitch and roll angle of vehicle

From above results, when scaled vehicle cross obstacle, ASC make an important effort to adjust the differential speed between right and left wheel, which can verify scaled vehicle body to be stability.

For the second experiment, we put two obstacles on the front route of the vehicle, the scaled vehicle follow path as shown in Fig. 3(b). The first pitch angle is about ${\theta }_1=$13.5° and $l_{r1}=$1.5 m distance from starting point. The second one is about ${\theta }_2=$23.5° and $l_{r2}=$2.8 m distance from starting point. The contrast experimental results of ASC and non-control (NC) are shown in following picture.

Fig. 6 is shown variation of pitch angle and average speed when scaled vehicle cross obstacles. If the vehicle was not controlled by active safety control algorithm, at first obstacle it can cross without any violent fluctuate of speed and pitch angle. But it fails to cross the second obstacle and tip-over in the end, which can be implied by pitch angle results. On the contrary, ASC control acceleration of scaled vehicle, pitch angle is adjusted and cross two obstacles successfully.

Fig. 6Contrast curve of Non-control (NC) and Active Safety Control (ASC)

a) Pitch angle contrast curve

b) Average speed contrast curve

5. Conclusions

In this paper, a 5-DOF dynamic rollover and tip-over model of orchard vehicle is established as theoretical model working for control method. After that, using Kalman prediction method, the active safety control algorithm is structured, which control 2-axis can accelerate and body posture of vehicle to prevent rollover and tip-over accident. Finally, scaled experimental system was set up, and scaled test prove feasibility and operability of algorithm.