Design and Test of a Levelling System for a Mobile Safflower Picking Platform

Abstract

At this stage, safflower picking is mostly performed manually or semi-manually, the picking method is antiquated and the picking precision is low. In this experimental study, a new attitude tilt levelling system was designed for a safflower-picking robot, which has created a solid foundation for the realization of future safflower-picking machine automation. The mobile platform was simplified as a four-point support, and an automatic levelling control system was designed based on the multi-sensor data collected by a multi-inclination sensor, a multi-pressure sensor, and a displacement sensor. The error range of the levelling of the mobile platform was obtained by MATLAB simulation analysis, the relationship between the inclination of the mobile platform and the displacement of the levelling mechanism was analyzed by coordinate transformation, and the maximum levelling range of the levelling mechanism was analyzed. On this basis, an automatic levelling control system was designed. Finally, the safflower-picking mobile platform was tested, and we concluded that the levelling control system can adjust the inclination angle of the mobile platform to within 0.2° and the levelling time to within 7 s. The design of the automatic levelling control system fills the gap in the field of safflower picking and adopts multi-sensor fusion. Compared with other methods, the collected inclination data is more accurate, the levelling accuracy higher, and the levelling time shorter. The final results show that this experimental study provides a strong basis for the realization of the full-mechanical automation of safflower picking.

1. Introduction

As a characteristic cash crop with strong drought and cold resistance, safflower can be used as a source of oil, dyes [1,2,3], natural pigments, and other supplies. There are large areas of agricultural land for planting safflower in various regions of the country. However, due to the different appearance characteristics of safflower plants and other crops, special safflower-picking machinery is needed to realize the full mechanization of safflower picking. Nowadays, the safflower picking method is still in full manual picking and semi-mechanical picking mode, so the current picking methods are still very antiquated. Nowadays, full mechanization of agricultural picking has become the trend of the times. It is necessary to research new safflower-picking robots, but the terrain of safflower picking is not flat, so it is easy for the mobile platform to tilt during the picking process. Since the robot arm is mounted on the mobile platform for the picking operation, the tilt of the mobile platform position will cause the three-dimensional coordinates of the robot arm to change, thus making the robot arm reach the wrong position to pick the safflower, thus affecting the picking accuracy. In order to ensure the accurate picking of safflower, the work performed by our mobile platform requires high precision [4]. Therefore, it is necessary to design an automatic levelling system suitable for picking in order to ensure the correct level of the robot arm’s position.

At present, the research on automatic safflower-picking machinery is nonexistent. In this research, the advantages of current mature agricultural automatic machinery levelling are combined to create a robot levelling mechanism especially suitable for safflower picking. According to the operating requirements of different field terrain, the levelling technology of agricultural machinery suitable for different environments has been widely used. Yu Zhicheng et al. designed an automatic levelling system for paddy field machinery that adopts hydraulic levelling [5]. After receiving a signal from an angle sensor, the controller controls the levelling of the frame by controlling the flow of hydraulic oil in an oil cylinder. However, the designed levelling system has a slow response speed and low sensor precision, and the mechanical levelling work begins only when the inclination exceeds 0.3°, the levelling error is 0.36°, and when the levelling accuracy is not high. LiuPingyi et al. [6] designed an agricultural chassis levelling mechanism for operation in hilly and mountainous areas that can realize multi-wheel drive and has a high levelling accuracy of approximately 1° during chassis walking. However, in the levelling test, in 90% of the cases, the levelling accuracy was about 0.5° floating, and the levelling accuracy was not high. Zhou Hao et al. [7] designed an automatic levelling device for a rotary tiller, which was composed of a rotary tiller mechanism, levelling support frame, a hydraulic system, and an automatic levelling control system, significantly improving the horizontal surface flatness after rotary cultivator tillage. The maximum horizontal height difference of the front ridge surface was 1.9 cm, and the maximum horizontal height difference of the tillage depth was 1.8 cm. After the rotary tillage mechanism was levelled, the height error of cultivated land was about 2 cm, and the levelling accuracy was not high. Moreover, the levelling mechanism used the hydraulic type and the improper operation of the hydraulic mechanism led to oil leakage, so there was no strict transmission ratio. At present, most existing levelling systems are used in agricultural machinery and agricultural machines and tools, and the accuracy is low, so they are not suitable for the field of picking robots. Therefore, to ensure safflower picking accuracy, a levelling system suitable for a mobile safflower picking platform needs to be designed. At present, the research on safflower-picking machinery is limited to the picking robot arm, and the safflower-picking robot mobile platform designed in this paper fills the gap in the field of safflower-picking mobile platforms. In order to solve the problem of the decrease in safflower picking accuracy caused by the positioning error caused by the tilt of the mobile platform when picking safflower by the manipulator, a levelling control system was added to the design of the mobile platform. A levelling control system was also the first application in safflower-picking machinery. In the selection of devices, multiple inertial measurement units (IMUs) fusion were used to obtain the tilt angle information of the mobile platform, which improved the accuracy of angle acquisition, which can be levelled in real time with high levelling accuracy and can ensure safflower picking accuracy.

In Section 2, the overall structure of the mobile platform and the working process of the levelling system are introduced. In Section 3, the measuring principle of the levelling mechanism–electric push rod, the levelling accuracy requirement of the levelling control system, and the maximum levelling angle range are introduced. In Section 4, the design of the levelling control system for the mobile platform is introduced, including hardware selection and software design. In Section 5, the designed automatic levelling control system is tested to further verify the feasibility of the designed levelling control system. In Section 6, the design process and test results of the levelling control system are summarized.

2. Overall Structure and Working Principle

2.1. Overall Design

In this research, we first made a new design for the whole safflower picking platform, and innovatively added four electric push rods to the four driving legs of the robot as part of the levelling system (as shown in Figure 1, No. 2). In addition, three IMUs were innovatively added as angle measuring tools, so as to ensure the accuracy of the measuring angle. The automatic mobile safflower picking platform levelling mechanism is composed of four independent electric push rods, a stepper motor driver, a controller, a CAN communication module (the model is MCP2515), one 9-degree-of-freedom (DOF) (IMU), two single-degree-of-freedom (DOF) (IMU), one cable displacement sensor, four pressure sensors, and a level gauge. The upper end of the electric push rod was then fixed to the box body of the mobile platform, and the lower end fixed to the wheel support plate through a square steel structure. The driver and electric push rod were connected by bolts. The nine-axis inclination sensor was installed at the center of the picking platform in order to measure the angle of the X-axis and Y-axis when the car body is inclined. The two single-axis inclination sensors were then installed vertically on the X-axis and Y-axis in order to measure the inclination of the X-axis or Y-axis separately. The pull wire displacement sensor was used to sense the telescopic length of the electric push rod. The level gauge was used to detect the levelling accuracy after levelling. The pressure sensor was used to detect the pressure of each outrigger and in order to ascertain whether there was a virtual leg. The tilt angle of the mobile platform is controlled by the expansion and contraction of the electric push rod in order to realize the levelling of the vehicle body. Each part of the levelling system is shown in Figure 1. In this design, the levelling parts are fully used, which greatly improves the accuracy of platform levelling.

2.2. Operational Principle

When the mobile platform works in the field, it will be affected by the field’s topography, resulting in the tilt of the mobile platform, and the tilt of the mobile platform posture will reduce the accuracy of safflower picking. At this time, the automatic levelling control system is needed to adjust the position and posture of the mobile platform to the horizontal state. The automatic levelling control system is an electric drive system based on multi-sensor fusion. The sensors are used to receive the tilt angle of the mobile platform and the pressure value of the electric push rod and transmit it to the controller in real time. According to the principle of levelling control, the designed automatic levelling control system is shown in Figure 2. The inclination sensor is installed in the center of the mobile platform in order to detect the inclination information when the mobile platform is tilted. The inclination information is transmitted to the controller through CAN communication. After receiving the information, the controller begins to calculate the height difference between each electric push rod and then controls the driver to drive the electric push rod for lifting action. The lifting and contraction of each electric push rod are realized through the start [8], stop, and positive and negative movements of the motor to accomplish automatic levelling of the mobile levelling table. When the position and posture of the mobile platform are horizontal, the height position of the electric push rod is obtained by observing the value of the displacement sensor installed on the first electric push rod. After levelling, the pressure sensor installed above the electric push rod detects the pressure value of each electric push rod and feeds back the pressure value to the controller. When the pressure value of the electric push rod is zero or significantly less than that of the rest of the electric push rod, the controller will execute the “virtual leg” elimination program. The reasons for designing the “virtual leg” elimination program are: when four-point support is adopted, one outrigger will not be stressed due to deformation and other problems, and the weight of the platform will be supported by the other three outriggers [9], resulting in virtual legs. When virtual legs occur in the levelling process, the mobile platform will shake due to external interference, and overturn will occur in serious cases. Therefore, it is necessary to avoid the imaginary appearance of empty legs.

3. Levelling Strategy and Analysis

3.1. Mathematical Modelling of the Mobile Platform

A plane is determined by two intersecting lines to establish the schematic diagram of the picking platform, as shown in Figure 3 [10]. The coordinate system OXYZ is established with the platform center as the coordinate origin O. The vertical upwards direction of the center point is the positive direction of the Z-axis, the horizontal rightward direction is the positive direction of the X-axis, and the horizontal forward direction is the positive direction of the Y-axis. The nine-axis inclination sensor is installed in the center of the platform, and the two single-axis inclination sensors are installed on the X-axis and Y-axis at the same distance from the nine-axis sensor, which are used to measure the inclination of the X-axis and Y-axis. The fixed-point method is used to calculate the telescopic length of the electric push rod. When the inclination sensor outputs zero X- and Y-axis inclination relative to the center point, the platform is horizontal. It is assumed that the distance between two adjacent outriggers is equal and is level with the fixed-point method, and the fixed point is P₁.

Three inclination sensors installed in the center of the mobile platform fuse the inclination information of the central point of the picking platform relative to the X-axis and Y-axis. After fusion, the angle of the X-axis inclination is α. When the Y-axis is tilted after fusion, the angle is β. When the platform is tilted, the transformation matrices of the mobile platform around the X- and Y-axes are, respectively:

$$\begin{gathered} {\mathrm{R}}_{\mathrm{X}}=\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathsf{\alpha }& -\sin \mathsf{\alpha }\\ 0& \sin \mathsf{\alpha }& \cos \mathsf{\alpha }\end{array}\right) \\ {\mathrm{R}}_{\mathrm{Y}}=\left(\begin{array}{ccc}\cos \mathsf{\beta }& 0& \sin \mathsf{\beta }\\ 1& 0& 1\\ -\sin \mathsf{\beta }& 0& \cos \mathsf{\beta }\end{array}\right) \end{gathered}$$

(1)

After the mobile platform rotates around the X- and Y-axis in space, the following is obtained:

$${{{\mathrm{R}}_1=\mathrm{R}}_{\mathrm{X}}\mathrm{R}}_{\mathrm{Y}}=\left(\begin{array}{ccc}\cos \mathsf{\beta }& 0& \sin \mathsf{\beta }\\ \sin \mathsf{\alpha }\sin \mathsf{\beta }& \cos \mathsf{\alpha }& -\sin \mathsf{\alpha }\cos \mathsf{\beta }\\ -\cos \mathsf{\alpha }\sin \mathsf{\beta }& \sin \mathsf{\beta }& \cos \mathsf{\alpha }\cos \mathsf{\beta }\end{array}\right)$$

(2)

When the mobile platform is not tilted, the coordinates of support points P₁, P₂, P₃, and P₄ are:

$$\left\{\begin{array}{cc}{\mathrm{P}}_1={\left(-\frac{\mathrm{a}}{2},-\frac{\mathrm{a}}{2},0\right)}^{\mathrm{T}}& {\mathrm{P}}_2={\left(-\frac{\mathrm{a}}{2},\frac{\mathrm{a}}{2},0\right)}^{\mathrm{T}}\\ {\mathrm{P}}_3={\left(\frac{\mathrm{a}}{2},\frac{\mathrm{a}}{2},0\right)}^{\mathrm{T}}& {\mathrm{P}}_4={\left(\frac{\mathrm{a}}{2},-\frac{\mathrm{a}}{2},0\right)}^{\mathrm{T}}\end{array}\right.$$

(3)

After the mobile platform rotates using rotation matrix R₁, the coordinates of support points P₁, P₂, P₃, and P₄ are transformed into:

$$\left\{\begin{array}{c}{\mathrm{P}}_1={\left(-\frac{\mathrm{a}}{2}\cos \mathsf{\beta },-\frac{\mathrm{a}}{2}\left(\sin \mathsf{\alpha }\sin \mathsf{\beta }+\cos \mathsf{\alpha }\right),\frac{\mathrm{a}}{2}\left(\cos \mathsf{\alpha }\cos \mathsf{\beta }-\sin \mathsf{\alpha }\right)\right)}^{\mathrm{T}}\\ {\mathrm{P}}_2={\left(-\frac{\mathrm{a}}{2}\cos \mathsf{\beta },\frac{\mathrm{a}}{2}\left(\cos \mathsf{\alpha }-\sin \mathsf{\alpha }\sin \mathsf{\beta }\right),\frac{\mathrm{a}}{2}\left(\cos \mathsf{\alpha }\cos \mathsf{\beta }+\sin \mathsf{\alpha }\right)\right)}^{\mathrm{T}}\\ {\mathrm{P}}_3={\left(\frac{\mathrm{a}}{2}\cos \mathsf{\beta },\frac{\mathrm{a}}{2}\left(\sin \mathsf{\alpha }\sin \mathsf{\beta }+\cos \mathsf{\alpha }\right),\frac{\mathrm{a}}{2}\left(\sin \mathsf{\alpha }-\cos \mathsf{\alpha }\cos \mathsf{\beta }\right)\right)}^{\mathrm{T}}\\ {\mathrm{P}}_4={\left(\frac{\mathrm{a}}{2}\cos \mathsf{\beta },\frac{\mathrm{a}}{2}\left(\sin \mathsf{\alpha }\sin \mathsf{\beta }-\cos \mathsf{\alpha }\right),\frac{\mathrm{a}}{2}\left(-\cos \mathsf{\alpha }\cos \mathsf{\beta }-\sin \mathsf{\alpha }\right)\right)}^{\mathrm{T}}\end{array}\right.$$

(4)

Then, the height of each of the four electric push rods after rotation is:

$$\left\{\begin{array}{c}{\mathrm{L}}_1=\sqrt{{\left(-\frac{\mathrm{a}}{2}\cos \mathsf{\beta }\right)}^2+{\left[-\frac{\mathrm{a}}{2}\left(\sin \mathsf{\alpha }\sin \mathsf{\beta }+\cos \mathsf{\alpha }\right)\right]}^2+{\left[\frac{\mathrm{a}}{2}\left(\cos \mathsf{\alpha }\cos \mathsf{\beta }-\sin \mathsf{\alpha }\right)\right]}^2}\\ {\mathrm{L}}_2=\sqrt{{\left(-\frac{\mathrm{a}}{2}\cos \mathsf{\beta }\right)}^2+{\left[\frac{\mathrm{a}}{2}\left(\cos \mathsf{\alpha }-\sin \mathsf{\alpha }\sin \mathsf{\beta }\right)\right]}^2+{\left[\frac{\mathrm{a}}{2}\left(\cos \mathsf{\alpha }\cos \mathsf{\beta }+\sin \mathsf{\alpha }\right)\right]}^2}\\ {\mathrm{L}}_3=\sqrt{{\left(\frac{\mathrm{a}}{2}\cos \mathsf{\beta }\right)}^2+{\left[\frac{\mathrm{a}}{2}\left(\sin \mathsf{\alpha }\sin \mathsf{\beta }+\cos \mathsf{\alpha }\right)\right]}^2+{\left[\frac{\mathrm{a}}{2}\left(\sin \mathsf{\alpha }-\cos \mathsf{\alpha }\cos \mathsf{\beta }\right)\right]}^2}\\ {\mathrm{L}}_4=\sqrt{{\left(\frac{\mathrm{a}}{2}\cos \mathsf{\beta }\right)}^2+{\left[\frac{\mathrm{a}}{2}\left(\sin \mathsf{\alpha }\sin \mathsf{\beta }-\cos \mathsf{\alpha }\right)\right]}^2+{\left[\frac{\mathrm{a}}{2}\left(-\cos \mathsf{\alpha }\cos \mathsf{\beta }-\sin \mathsf{\alpha }\right)\right]}^2}\end{array}\right.$$

(5)

To keep the mobile platform level, the height of each push rod is:

$$\left\{\begin{array}{c}{\mathrm{P}}_2={\mathrm{L}}_2-{\mathrm{L}}_1\\ {\mathrm{P}}_3={\mathrm{L}}_3-{\mathrm{L}}_1\\ {\mathrm{P}}_4={\mathrm{L}}_4-{\mathrm{L}}_1\end{array}\right.$$

(6)

After the fusion of the three inclinations of sensors α and β, the length of the electric push rod relative to the fixed push rod can be obtained by the controller.

3.2. Levelling Accuracy Analysis

The analysis of levelling accuracy is an important link in the design of levelling structures. The mobile platform is the carrier carried by the manipulator. When the posture of the mobile platform is tilted, the fixed platform of the manipulator is tilted, so the three-dimensional coordinates of the end effector of the manipulator are changed, resulting in a decline in picking accuracy. When the position error of the end actuator of the picking manipulator is <5 mm, the safflower can be accurately picked. Therefore, the levelling accuracy of the mobile platform determines the picking accuracy of the manipulator. The levelling error range of the mobile platform is determined by the reverse calculation of the picking accuracy error of the picking manipulator. γ is the rotation angle of the mobile platform about the Z-axis; then, the rotation matrix of the mobile platform about the Z-axis is:

$${\mathrm{R}}_{\mathrm{Z}}=\left(\begin{array}{ccc}\cos \mathsf{\gamma }& -\sin \mathsf{\gamma }& 0\\ \sin \mathsf{\gamma }& \cos \mathsf{\gamma }& 0\\ 0& 0& 1\end{array}\right)$$

(7)

Then, the rotation matrix of the mobile platform around the X-, Y-, and Z-axes

$${\mathrm{R}}_2={\mathrm{R}}_{\mathrm{X}}{\mathrm{R}}_{\mathrm{Y}}{\mathrm{R}}_{\mathrm{Z}}=\left(\begin{array}{ccc}\cos \mathsf{\beta }\cos \mathsf{\gamma }& -\cos \mathsf{\beta }\sin \mathsf{\gamma }& \sin \mathsf{\beta }\\ \sin \mathsf{\alpha }\sin \mathsf{\beta }\cos \mathsf{\gamma }+\cos \mathsf{\alpha }\sin \mathsf{\gamma }& \cos \mathsf{\alpha }\cos \mathsf{\gamma }-\sin \mathsf{\alpha }\sin \mathsf{\beta }\sin \mathsf{\gamma }& -\sin \mathsf{\alpha }\cos \mathsf{\beta }\\ \sin \mathsf{\beta }\sin \mathsf{\gamma }-\cos \mathsf{\alpha }\sin \mathsf{\beta }\cos \mathsf{\gamma }& \sin \mathsf{\beta }\cos \mathsf{\gamma }+\cos \mathsf{\alpha }\sin \mathsf{\beta }\sin \mathsf{\gamma }& \cos \mathsf{\alpha }\cos \mathsf{\beta }\end{array}\right)$$

(8)

The end effector of the manipulator and the mobile platform use the same coordinate system, so the three-dimensional coordinates of the end effector of the manipulator can be obtained after the rotation matrix transformation. The levelling accuracy is calculated using MATLAB simulation software, and the levelling accuracy range of the platform is within 0.2°.

When the error of the moving platform is within 0.2°, the error of the end actuator of the manipulator is within 5 mm. The error of the end effector of the manipulator is found to be within 5 mm through the forward solution of the determined levelling accuracy. It can be determined that the obtained accuracy meets the requirements. Since the levelling of the mobile platform is based on the rotation of the X- and Y-axes, the transformation of the mobile platform about the Z-axis is not considered. The levelling accuracy analysis of the mobile platform is an important parameter to evaluate whether the mobile platform has completed the levelling task.

3.3. Levelling Limit Range

The mobile platform is only levelled about the X- and Y-axes, so the maximum levelling angle of the mobile platform about the X- and Y-axes and diagonal is calculated as follows, with the equivalent three-dimensional rotation shown in Figure 4.

a

When the X-axis is inclined, P₁ and P₂ outriggers rise to the highest point. At this time, L_max = 0.2 m, and d_X = 1 m. According to the trigonometric function, the maximum allowable inclination angle at this time is:

$$\angle \mathsf{\alpha }={\sin }^{-1}\left(\frac{{\mathrm{L}}_{\max }}{{\mathrm{d}}_{\mathrm{X}}}\right)={\sin }^{-1}\left(0.2\right)$$

(9)

b

When the Y-axis is inclined, the P₂ and P₃ outriggers rise to the highest point. At this time, L_max = 0.2 m and d_X = 1 m. According to the trigonometric function, the maximum allowable inclination angle at this time is:

$$\angle \mathsf{\beta }={\sin }^{-1}\left(\frac{{\mathrm{L}}_{\max }}{{\mathrm{d}}_{\mathrm{Y}}}\right)={\sin }^{-1}\left(0.2\right)$$

(10)

c

When inclination occurs on the diagonal, the P₃ outrigger rises to the highest point, and the P₁ outrigger height remains unchanged. At this time, L_max = 0.2 m, and d_X-_Y = 2^1/2 m. According to the trigonometric function relationship, the maximum allowable inclination angle at this time is:

$$\angle \mathsf{\delta }={\sin }^{-1}\left(\frac{{\mathrm{L}}_{\max }}{{\mathrm{d}}_{\mathrm{X}-\mathrm{Y}}}\right)={\sin }^{-1}\left(\frac{{\mathrm{L}}_{\max }}{\sqrt{{{\mathrm{d}}_{\mathrm{X}}}^2+{{\mathrm{d}}_{\mathrm{Y}}}^2}}\right)={\sin }^{-1}\left(\frac{0.2}{\sqrt{2}}\right)$$

(11)

By calculating the maximum theoretical levelling angle of the mobile platform, the maximum levelling tilt angle of the mobile platform in three directions can be obtained. According to the trigonometric function formula, the maximum allowable inclination angles of the mobile platform in three directions are 11.54°, 11.54°, and 8.13°. Calculating the maximum levelling angle of the mobile platform is an important parameter to ensure that the mobile platform can complete the levelling angle limit range [11].

4. Control System Design

4.1. Hardware Design

The automatic levelling system of the mobile platform is composed of a picking platform, controller, stepping motor driver, electric push rod, IMU, displacement sensor, pressure sensor, Voltage amplifying module (the model is HX711), level meter, and CAN communication module. The controller is the core of the levelling system [12] (which is a node in the control of the entire mobile platform. The controller chip is an Arduino Mega. It has enough memory to meet the levelling requirements.

IMU adopts a 9-degree-of-freedom (DOF) IMU (the model is HWT901B) and a single-degree-of-freedom (DOF) IMU (the model is SINDT01), which are produced by Shenzhen Weite Intelligent Company, China. The HWT901B 9-degree-of-freedom (DOF) IMU and the SINDT01 single-degree-of-freedom (DOF) IMU can achieve static accuracy within 0.05° and dynamic levelling within 0.1°. Because the automatic levelling control system adopts three IMUs fusion, in order to facilitate the data transmission, the CAN communication module (the model is MCP2515) is adopted, since using the CAN module can reduce the connection between devices and the transmission pressure of the main controller. Therefore, the communication mode between the IMUs and the controller is CAN communication. The displacement sensor is selected to observe the displacement of each electric push rod after levelling. The linear accuracy of the displacement sensor is ±0.15%, which can accurately observe the displacement of the electric push rod. Because the output signal of the pressure sensor is mV, in order to facilitate the controller to receive, the voltage amplification module (model HX711) is selected to cooperate with the pressure sensor, so that the mV signal can be amplified to the signal range that the controller can receive. The communication method between the displacement sensor, pressure sensor, and controller is serial communication. The levelling mechanism adopts an electric push rod, which has high precision and fast-moving speed. However, the electric push rod cannot communicate directly with the controller, so it needs to be equipped with a driver. After receiving the pulse signal from the controller, the driver will convert the pulse signal into an electrical signal, thus driving the electric push rod to rotation. The rotation of the electric push rod makes the electric push rod perform a lifting action. Through the number of pulses emitted by the driver, the angular displacement of the rotation of the electric push rod can be controlled, and the speed of the rotation of the electric push rod can be controlled through the pulse frequency emitted by the driver. Whether the pulse emitted by the driver is high or low, it controls the direction of the rotation of the electric push rod, and the precise control of the displacement of the electric push rod can be realized by driving the driver. The circuit connection between the electric actuator, various sensors, and the controller is shown in Figure 5.

4.2. Software Design

The core of the control system is software. The design of the software system affects the speed and precision of the levelling system [13]. The software of the control system is developed using the Arduino Integrated Development Environment (IDE) platform. The software programming is based on the C and C++ languages. The levelling algorithm is a fuzzy Proportional, Integral, Differential (PID) algorithm. The PID algorithm takes the angle error and angle error rate of the inclination sensors as the input [14], outputs the integral and differential proportion, and then looks for the appropriate value to make the angle output of the sensor more accurate. Before PID, it is necessary to obtain the inclination data in a single IMU and fuse the inclination data of the three IMUs. The Digital Motion Processor (DMP) inside the IMU can output quaternions directly, and data are converted into angle values [15]. The IMU has its own Kalman filter algorithm, which can filter the angle data, and filtering can effectively remove noise, including Quantization noise (QN), Angle random walk (ARW), Bias instability (BI), Rate random walk (RRW), and Rate ramp (RR) so that the fused angle output is more accurate. The formula for converting the angle value of the quaternion in a single IMU is as follows:

$$\mathsf{\beta }={\sin }^{-1}{\mathrm{g}}^1,\mathsf{\alpha }={\tan }^{-1}\frac{{\mathrm{g}}^2}{{\mathrm{g}}^3},\mathsf{\gamma }={\tan }^{-1}\frac{{\mathrm{g}}^4}{{\mathrm{g}}^5}$$

(12)

where: g1 g2 g3 g4 g5 is the input of α, β, and γ angles.

g1 = 2(q1q3 − q0q2)

g2 = 2(q2q3 + q0q1)

g3 = q0² + q3² − q2² − q1²

g4 = 2(q1q2 + q0q3)

g5 = q0² + q1² − q2² − q3²

The batch estimation fusion algorithm is used to fuse the inclination values converted by the three IMUs. This algorithm can avoid a fusion result error caused by the error of multiple sensors [16]. In the levelling process of the mobile platform, two sensors detect the inclination of the X-axis, and two sensors detect the inclination of the Y-axis. They are divided into two groups for angle detection. Taking the inclination angle of the X-axis as an example, X₁ = {x₁₁, x₁₂, x₁₃, x₁₄,…, x_1n} is the inclination information detected by the 9-degree-of-freedom (DOF) inertial measurement unit (IMU), and X₂ = {x₂₁, x₂₂, x₂₃, x₂₄,…, x_2n} is the inclination information detected by the single-degree-of-freedom (DOF) inertial measurement unit (IMU). Then, the average value of the X₁ inclination is $\stackrel{-}{{\mathrm{X}}_1}=\frac{1}{\mathrm{n}}\sum _{\mathrm{i}}^{\mathrm{n}}{\mathrm{x}}_{1\mathrm{n}}$, the average value of the X₂ inclination is $\stackrel{-}{{\mathrm{X}}_2}=\frac{1}{\mathrm{n}}\sum _{\mathrm{j}}^{\mathrm{n}}{\mathrm{x}}_{2\mathrm{n}}$, and the mean square deviation is: ${\mathsf{\sigma }}_1=\sqrt{\frac{1}{\mathrm{n}-1}\sum _{\mathrm{i}=1}^{\mathrm{n}}{\left({\mathrm{x}}_{1\mathrm{i}}-\stackrel{-}{{\mathrm{X}}_1}\right)}^2}$. and ${\mathsf{\sigma }}_2=\sqrt{\frac{1}{\mathrm{n}-1}\sum _{\mathrm{j}=1}^{\mathrm{n}}{\left({\mathrm{x}}_{1\mathrm{j}}-\stackrel{-}{{\mathrm{X}}_2}\right)}^2}$.

If the mean square deviation of the previous detection information is (σ_-) = ∞, then (σ_-)⁻¹ = 0. The fusion result of the batch estimation algorithm is:

$$\begin{gathered} {\mathrm{x}}^{+}=\left[{\mathsf{\sigma }}_{+}{\left({\mathsf{\sigma }}_{-}\right)}^{-1}\right]{\mathrm{x}}^{-}+\left[{\mathsf{\sigma }}_{+}{{\mathrm{H}}^{\mathrm{T}}\left(\mathrm{R}\right)}^{-1}\right]\mathrm{x}=\left[{\mathsf{\sigma }}_{+}{{\mathrm{H}}^{\mathrm{T}}\left(\mathrm{R}\right)}^{-1}\right]\mathrm{x} \\ {\mathsf{\sigma }}_{+}={\left[{\left({\mathsf{\sigma }}_{-}\right)}^{-1}+{{\mathrm{H}}^{\mathrm{T}}\left(\mathrm{R}\right)}^{-1}\mathrm{H}\right]}^{-1} \\ \mathrm{H}=\left(\begin{array}{c}1\\ 1\end{array}\right), \mathrm{x}=\left(\begin{array}{c}\stackrel{-}{{\mathrm{x}}_1}\\ \stackrel{-}{{\mathrm{x}}_2}\end{array}\right), \mathrm{R}=\left(\begin{array}{cc}{{\mathsf{\sigma }}_1}^2& 0\\ 0& {{\mathsf{\sigma }}_2}^2\end{array}\right) \end{gathered}$$

(13)

where x⁻ is the last data fusion result; x is the arithmetic mean matrix; σ₊ is the variance of the data fusion results; H is the coefficient matrix of the measurement equation; and R is the covariance matrix of the measurement noise.

Will (σ_-)⁻¹ = 0 is substituted to obtain:

$${\mathrm{x}}^{+}={\left[\sum _{\mathrm{i}=1}^2\frac{1}{{{\mathsf{\sigma }}_{\mathrm{i}}}^2}\right]}^{-1}·\sum _{\mathrm{i}=1}^2\frac{\stackrel{-}{{\mathrm{x}}_{\mathrm{i}}}}{{{\mathsf{\sigma }}_{\mathrm{i}}}^2}{\mathsf{\sigma }}_{+}={\left[\sum _{\mathrm{i}=1}^2\frac{1}{{{\mathsf{\sigma }}_{\mathrm{i}}}^2}\right]}^{-1}$$

(14)

x⁺ is the inclination value after fusion, and the inclination value after fusion is closer to the real value.

The levelling of the mobile platform is divided into automatic levelling and manual levelling. The automatic levelling process is a continuous cyclic process. IMU will constantly detect the inclination data. When the mobile platform is tilted, it will achieve automatic levelling by controlling the lifting of the electric drive. Through the fixed-point method, an automatic levelling control system is designed, which specifies that the position of the first electric push rod is fixed and does not participate in the levelling process. The automatic levelling control process is as follows: IMUs obtain the inclination information of the mobile platform and carry on the fusion, and the fused information is transmitted to the controller through CAN communication. After receiving the information, the controller begins to calculate the height difference of each electric push rod relative to the first electric push rod, so as to make a control decision. At this time, the driver begins to drive the electric push rod to participate in the levelling process to achieve the level of the position and posture of the mobile platform. Manual levelling is controlled by switches. There are thirteen switches in total, which control the raising and lowering of the four electric push rods, raising and lowering at the same time, and performing an emergency stop. When any switch is pressed, a push rod will perform the corresponding action. The pressure sensor is installed on the four outriggers. After levelling, the stress in each outrigger is detected, and the outrigger with the virtual leg is adjusted to eliminate the virtual leg. The flow chart of outrigger automatic levelling is shown in Figure 6 and virtual leg detection is are shown in Figure 7, and the manual levelling button settings are shown in

Table 1. Switch signal input.

Manual Levelling Button Label	Movement Trend	From
0	Manual/Automatic	Controller
1	1 Up	Linear actuator1
2	2 Up	Linear actuator 2
3	3 Up	Linear actuator 3
4	4 Up	Linear actuator 4
11	1 Down	Linear actuator1
22	2 Down	Linear actuator 2
33	3 Down	Linear actuator 3
44	4 Down	Linear actuator 4
5	All up	Linear actuators 1, 2, 3, 4
6	All down	Linear actuators 1, 2, 3, 4
7	Reset	Linear actuators 1, 2, 3, 4
8	Cease	Linear actuators 1, 2, 3, 4

.

5. Test and Result Analysis

To verify the feasibility of the designed levelling system and virtual leg elimination system, the safflower-picking mobile platform was tested. The designed levelling mechanism was installed on the mobile platform of the safflower picking robot for field experiments and commissioning. The test location was Hongqi Farm in Jimsar County, Changji Autonomous Prefecture, Xinjiang Uygur Autonomous Region. Figure 8 is the field test of the mobile platform. a is the overall structure diagram of the mobile platform, b is the installation position of the electric push rod, and c is the installation position of the IMU. The field test of the mobile platform is shown in Figure 9. Figure 9a shows the tilt attitude of the mobile platform before levelling, and Figure 9b shows the attitude of the mobile platform after levelling.

The levelling of the mobile platform was divided into static levelling and dynamic levelling. The X-axis and Y-axis angles when the body is tilted were displayed through the serial port assistant, and the levelling time was recorded. Static levelling manually changes the inclination of the mobile platform so that the mobile platform is in eight tilting states: front lowest, rear lowest, left lowest, right lowest, left front lowest, right front lowest, left rear lowest, and right rear lowest. Then, these eight states were automatically and manually levelled, the levelling tilt angle recorded, and the automatic levelling curve and the time required for manual and automatic levelling drawn, as shown in Figure 10. Figure 11 shows the comparison between automatic levelling time and manual levelling time. Figure 12 is the performance index diagram of the test results. The red points is the angle of α and β after leveling the mobile platform.It can be seen that the dip angle after levelling is in the range of 0.2°, which meets the requirements of levelling accuracy. The tilt angle of the mobile platform before and after the levelling test is shown in

Table 2. Results of the levelling test.

Tilt State	before Levelling		after Levelling		Levelling Time(s)
	α	β	α	β
Front lowest	−4.806°	0.84	−0.192°	0.15°	7.1
Rear lowest	4.86°	0.56°	0.168°	0.18°	7.3
Left lowest	0.724°	−5.25°	0.124°	−0.151°	7.6
Right lowest	0.783°	5.51°	0.179°	0.157°	8
Left front lowest	−4.31°	−5.34°	0.162°	−0.172°	7
Right front lowest	−5.76°	4.56°	−0.184°	−0.141°	5
Left rear lowest	5.58°	−5.71°	0.147°	−0.158°	5.2
Right rear lowest	5.31°	5.16°	0.146°	0.195°	7.3

.

For dynamic levelling, the automatic levelling mode is selected, and the scope of a section of the test field is delimited. The mobile platform carries out the nonadjustable parallel driving test and the driving while levelling the test at the same speed to test the inclination change and the levelling effect of the mobile platform when it passes this section of the road. The test is divided into three parts, and the recorded levelling data are shown in Figure 13a–c are the curve of the change of inclination angle in the process of three dynamic leveling respectively. After levelling, the pressure sensor detects the pressure value of each outrigger. If the pressure value of one outrigger is approximately 0, which is less than the pressure value of the other outriggers, it starts to eliminate the virtual leg until the pressure value of the four outriggers reaches a relatively average value.

Figure 10 shows that both automatic levelling and manual levelling can level the mobile platform, and the levelling accuracy is 0.2°. Due to different initial angles of inclination, automatic levelling can level the platform within 7 s, and manual levelling can level the platform within 11 s. This is fast enough for the targeted application. The automatic levelling system takes less time than manual levelling and has a faster response time. During the levelling process, the inclination of the mobile platform in the X- and Y-directions decreases. After levelling, the tilt difference of the mobile platform in the X- and Y-directions is very small and within the levelling error range. At the same time, the smaller the initial inclination of the mobile platform is, the shorter the total levelling time. Figure 13 shows that during the dynamic levelling process, due to the subsequent lifting effect of the electric push rod, the tilt angle of the mobile platform during driving is improved, and the angle after levelling is significantly smaller than that before levelling. However, due to the short and continuous tilt time of the mobile platform during driving, the tilt of the mobile platform cannot be fully adjusted within the accuracy range. Therefore, the designed levelling mechanism of the safflower-picking mobile platform has a good levelling effect and can meet the requirements of safflower picking.

6. Conclusions

According to the requirements of accurate safflower picking, an automatic levelling control system based on a safflower-picking mobile platform was designed, in which multiple sensors were fused to detect the tilt angle of the mobile platform, and pressure sensors were used to eliminate the “virtual leg” after levelling. The calculation formula between the inclination angle of the mobile platform and the displacement of the electric push rod was analyzed by using the coordinate transformation method, and the maximum angle allowed to tilt in the X, Y, and diagonal directions of the mobile platform was analyzed, as shown in Figure 4. The maximum allowable inclination angles of the mobile platform in the X, Y, and diagonal directions were 11.54°, 11.54°, and 8.13°.

The design of the automatic levelling control system was tested, as shown in Figure 10 and Figure 11. Figure 10 shows that no matter what tilt the mobile platform is in, the position and posture of the mobile platform can return to the horizontal state after levelling, and the accuracy after levelling is within 0.2°, which meets the requirements of the accurate picking of the safflower. Figure 11 shows the change of automatic levelling time and manual levelling time, which is due to manual adjustment, which requires one to pay attention to the inclination value of the mobile platform at any time and needs to be adjusted repeatedly, so it should be noted that the position and attitude adjustment of the mobile platform cannot be achieved in one step. Figure 13 shows the result of the dynamic levelling of the mobile platform. Although the posture of the mobile platform cannot be adjusted to the horizontal state in a short time, it has obviously improved.

In this paper, the automatic levelling control system was designed to obtain the inclination angle of the mobile platform by the fusion of three IMUs and eliminate “virtual legs” after levelling. Compared with the automatic levelling control system designed by [5,6,7], the accuracy has improved, and it also improved the overall stability of the mobile platform. In this paper, the design of the safflower picking automatic levelling control system fills the gap in the field of safflower picking. Further research should focus on the control algorithm of the automatic levelling system in order to improve the levelling accuracy and shorten the levelling time.