Trajectory planning and motion control of full-row seedling pick-up arm

: Frequent start-stops of automatic seedling transplanters would lengthen the seedling picking time due to alternating movement of the motor, and cause positioning errors and frequent motor vibrations when starting the machine. To solve these problems and ensure the pick-up continuity of automatic transplanters, in this study the smooth circular arc interpolation algorithm and the least square method for multinomial curve fitting were used to conduct the trajectory planning of automatic transplanter based on the movement of the pick-up arm of the transplanter. Velocity and time curves were fitted in segments to acquire motion control parameters and further tracking control was conducted. The mathematical model of the control system was established using Simulink, and simulation analysis and system debugging test were performed. Experiments show that the trajectory curves obtained by MATLAB’s data processing can realize the continual and smooth motion trajectory of the pick-up arm; stepper motor velocity control can effectively track the planning curve and improve the seedling pick-up efficiency. The pick-up and pushing time of each motion of the planned plug seedling transplanter reduced by 1.0678 s and the start-stop times reduced from 6 to 1, which solved the frequent motor vibrations when starting the machine and improved the stability of the system. In addition, overstep and out-of-step of the motor at the start-stop moment were avoided and displacement error was reduced.


Introduction
Seedling transplantation is a good way to reduce the impact of storms, rains, snow, low temperature and other natural disasters on the crops during its seedling stage. Timely transplanting can ensure the number of plants per hectare, improve yields and save seeds. Seedling transplantation has been a critical process for pepper and tomato planting in Xinjiang, owing to its ability to use light and heat resources [1] . Current transplantation is basically realized manually or semi-automatically, which requires great labor with low efficiency [2,3] , so fully automatic transplanter with automatic seedling pick-up and dropping mechanism becomes the research trend. Pearson Automatic Transplanter (England) and Williames Automatic Transplanter (Australia) make accurate pick-up and dropping of seedlings by controlling the position of plug seedlings through vertical and horizontal pushing and positioning system [4,5] .
These transplanters push the plug seedlings (hard plug) in rows with seedling pushing rod or pick out the plug seedlings in rows with needle-type seedling pick-up arm. The dry land transplanter designed in Italy can realize the turning over, positioning and dropping of full-row seedlings. After the pick-up mechanism gets out the plug seedlings, the full-row pick-up arm would turn over the seedlings and drop them into the seedling cup, and the seedling pick-up and dropping process would be finished after the turnover by Programmable Logic Controller (PLC) solenoid valve drive cylinder [1,6] . Wei et al. [7] developed an automatic motion coordination and control system for plug seedling transplanting. This system is designed to coordinate and control the motion of the seedling pick-up mechanism, dropping mechanism, planting mechanism and transplanting mechanism. The system has realized the automatic control of pick-up and the dropping of plug seedlings. But the process is not continual. The motor needs frequent start-stops, causing vibrations during the starting moment. Besides, the pick-up and dropping process takes long, and the stability and working efficiency of the system are also unsatisfactory. Xiao et al. [8] used an improved arc transition path planning algorithm based on tolerance to plan the tool path of numerical control machine tools, which realized the tool path speed planning and interpolation, and improved the transition speed between track segments and the efficiency of complex surface machining. Niu [9] used the circular interpolation algorithm to plan the moving track of the end actuator of the palletizing machine, which reduced the running time and improved the moving efficiency. In these studies, the arc interpolation algorithm is applied to the tool path and the path planning of a palletizing robot, which achieved better control effect. In this paper, the circular interpolation algorithm is used in the movement control of the whole row of seedling picking manipulator to realize the fast and stable picking and delivering of the whole row of processing tomato bowl seedlings.
The movement of the full-row pick-up arm of the transplanter is discontinuous both vertically and horizontally as the discontinuities shown in the movement curve of the pick-up arm. But the motor needs frequent start-stops and the seedling pick-up time is long. To solve these problems, this paper used driving motors in both vertical and horizontal directions and adopted a time-sharing method for the motion trajectory planning of seedling pick-up arm [10,11] . In this paper, the potted seedlings of processing tomato in Xinjiang were used as transplanting objects. The motion trajectory curve was planned using smooth circular arc interpolation algorithm [12,13] ; trajectory parameters were determined by stepper motor velocity tracking control method [14] ; system modeling on the two-phase hybrid stepper motor and driving model was conducted, followed by simulative analysis and system debugging test to further improve seedling pick-up efficiency and stability.

Motion analysis of seedling pick-up mechanism
Automatic full-row plug seedlings pick-up mechanism is composed of rack, full-row pick-up arm, positioning mechanism, receiving belt and supply table, etc. The full-row pick-up arm is a gantry framed two-axis Cartesian system, including horizontal orbit and vertical orbit. The pick-up arm is installed on the beam, and the supply table and receiving belt are located at the two ends of the horizontal orbit, as shown in Figure 1 [6] . The dimension of the seedling tray is 540 mm × 280 mm × 42 mm, each tray with 128 plugs (16×8). The size of each plug is 32 mm (upper length) ×14 mm (bottom length)×45 mm (height), basically, a four edges pyramid shape, as shown in Figure 2.
The working process of the full-row pick-up arm is presented in Figure 3. Its horizontal and vertical movement is controlled by the horizontal motor and the lifting motor respectively. The rotary movement of the stepper motor is converted to linear movement by ball screw [15] . The picking process is as following: 1) the pick-up arm picks seedlings and lifts as the lifting stepper motor rotates in the forward direction, then stops at designed height; 2) the pick-up arm carries the seedlings to the above of the dropping position as the horizontal stepper motor rotates in the forward direction; 3) the pick-up arm goes down to Dropping Position 1 as the lifting motor rotates in the reverse direction, and drops the seedlings at Position 1 [16] ; 4) the pick-up arm goes back the same way to the above of the pick-up position. The tray driving motor pushes the row ahead and the full-row pick-up arm is ready for picking seedlings of next row. This time it drops the seedlings at Position 2.  This pick-up and dropping method is realized by fixed point pick-up and alternant dropping, with one fixed pick-up position and two dropping positions. Specifically, after the first row of seedlings was dropped at Position 1, the tray would go forward for one row, and the pick-up arm would go back to the original position for the pick-up of the second row of seedlings which would be dropped at Position 2. In this process, the pick-up arm is required to make reciprocating movement between one pick-position and two dropping positions to realize the pick-up and dropping of plug seedlings. The movement of the pick-up arm, which is "up-right-down" after pick-up and "up-left-down" after dropping, formed two rectangular trajectories as shown in Figure 3. During the vertical movement, only the lifting motor works. But when the pick-up arm reaches the above of the pick-up position, the lifting motor stops and the horizontal motor starts to work, moving the pick-up arm to the right until it gets to the location above Position 1. After that, the horizontal motor stops and the lifting motor drives the pick-up arm downwards. There are two discontinuity points (A and B) in a pick-up and dropping process. At point A, the lifting motor stops and the horizontal motor works; at point B, the horizontal motor stops and the lifting motor works. The movement is shown in Table 1. Four start-stops happened to the lifting motor and two start-stops happened to the horizontal motor in one pick-up and dropping motion. The start-stop would cause vibration and influence the working stability of the system. Meanwhile, the out-of-step and overstep of the motor during the start-stop will result in displacement error, which means the position of the pick-up arm could be inaccurate.

Determination of trajectory planning parameters
The effective motion of the full-row pick-up arm formed two rectangular trajectories on a two-dimensional surface. The two rectangular trajectories were optimized to find the most appropriate path from the start to the end. The specific points on the plane were first fixed and then the trajectory planning was made according to the fixed points.
As shown in Figure 4, the pick-up arm grabs the plug seedlings, goes up 4.2 cm vertically, moves to the above of the Dropping Position 1 along Trajectory 1 for seedling dropping, then returns to the above of pick-up position along Trajectory 1 waiting for next turn of pick up [17] . The pick-up arm stretches to the tray, grabs plug seedlings, goes up 4.2 cm vertically, moves to the above of the Dropping Position 2 along Trajectory 2 for seedling dropping, then returns to the above of original position along Trajectory 2. Thus a full process of pick-up and dropping motion is completed. So, one pick-up and dropping process involves two trajectories, Trajectory 1 and Trajectory 2. The distance between the centers of two seedling receiving belts is 9.6 cm, and the distance between the center of the first row tray and the center of the Receiving Belt 1 is 11.5 cm. The above surface of the grid month of receiving belt and the tray mouth are on the same plane. The motion trajectories and major parameters of the pick-up arm are shown in Figure 4.

Trajectory planning
A rectangular plane coordinate system was established. The center of the tray bottom, i.e. the O point in Figure 4, is the origin, the vertical direction is the Y-axis, and the horizontal direction is the X-axis. Select the tray bottom center (0, 0), the tray exit coordinate (0, 0.042), the dropping coordinate of the Receiving Belt 1 (0.115, 0.042), and set the center of the trajectory center as 0.01 m to achieve the highest coordinate of motion (0.0575, 0.052). Select the coordinates of the three fixed points: the start point, the highest point, and the end point, as shown in Table 2. Interpolation operation was conducted using smooth circular arc interpolation algorithm to calculate the coordinate of the interpolation point. "Interpolation" means to "add" or "insert" several mid-points between the start point and end point along a determined outline.
The coordinate of the interpolation points was calculated through interpolation operation using smooth circular arc interpolation algorithm. Interpolation operation is to add several mid-points between the start point and end point of a certain outline, i.e. to "insert" or "add" the coordinates of motion mid-points. This operation is a densification of data points [18,19] . In this paper, equal angular displacement was adopted for circular interpolation. One coordinate can be obtained through the equal angular displacement of the same direction, and all the interpolation coordinates can be obtained after several repetitions.
(1) Formulate a circle based on the above three coordinates. The coordinate of the center of the circle is (x 0 , y 0 ), the radius is r, and the equation of the circle is: (x -x 0 ) 2 + (y -y 0 ) 2 = r 2 (1) Randomly select three points on this circle, which were (x 1 , y 1 ), (x 2 , y 2 ), (x 3 , y 3 ), then: (3), and Equation (2) minus Equation (4), simplify the results and we can get: The only condition for the solution of x 0 , y 0 is that the coefficient determinant is not 0: After calculation with above method, we can get: coordinate of the circle center = (0.0575, -0.1183125), and r = 0.1703125 m.
(2) Calculate the angle degree of the circle center based on the given coordinates of the three points, circle radius and position of circle center.
Re-establish the rectangular plane coordinate system with the obtained circle center and get three coordinates: (-0.0575, 0.1603125), (0, 0.1703125), (0.0575, 0.1603125). The central angle of the arc connected by the three coordinates is 39.46323 o , as shown in Figure 5.
(3) Use smooth circular arc interpolation algorithm to calculate the coordinate of interpolation points.
Assume the angular displacement ∆θ in the time T s ; t s is the where, N is interpolation steps; is the central Angle which is 39.46323°; ∆θ is angular displacement.
According to Equation (9), the total number of interpolation steps is 12.
Calculate the interpolation coordinate according to Equation (10) and Equation (11). (x i , y i ) was the coordinate of the start point, which was selected as (0.0575, 0.1603125). 1 cos sin Equation (12) shows whether it comes to the interpolation end. If θ i+1 ≤ , carry on the interpolation; if θ i+1 > , correct the length ∆θ of the last step, which was expressed as ∆θ', ∆θ'= -θ i , and we get the interpolation coordinates as shown in Table 3.
(4) Based on the above interpolation coordinates, use a polynomial curve fitting to make the trajectory planning and MATLAB for calculation and curve simulation.
Use MATLAB and "polyfit" function to conduct polynomial curve fitting.
The mathematical foundation is the basic mathematical least square curve fitting principle, which constructs an analytic function through the invoking function, making the originally discrete points as close as possible to the given value. Determine the order number of the polynomial fitting and get the trajectory curve after fitting.
Basically, the principle of determining the order number of polynomial fitting should not exceed the given number of coordinates. The fitting curve is not the only one if the number of coordinates equals or exceeds the original number [20] .
The expression to select the fourth order fitting curve is: The trajectory curve is shown in Figure 6. In all the fitting curves, the dotted line is the trajectory curve after fitting, the broken line is the connecting line of original coordinates.  As shown, the fourth order polynomial fitting curve is basically the same as the connecting line of the original coordinates. The fitting degree is very high.

Analysis of motion process
The functions and graphs were obtained through the interpolation algorithm and curve fitting. The trajectory curve was realized by the resultant motion in both horizontal and vertical directions. The motion in each direction was a time function.
The motion trajectory was divided into the displacement functions in the horizontal and vertical directions.
The horizontal displacement distance of the trajectory is 0.115 m, and the vertical displacement is 0.01 m. In order to realize a highly efficient seedling pick-up, the horizontal movement should be completed in the shortest time at the maximum acceleration. The horizontal movement first reaches the highest velocity at the maximum acceleration, keeps moving at this velocity, then slows down at the maximum acceleration until the pick-up arm stops at the dropping position. As the movement distance in the vertical direction was very short, the displacement trajectory was fitted by adjusting the velocity in the vertical direction [21] .
The stepper motor was chosen as the control target. The rotational displacement was converted to linear displacement through the connection with the ball screw. The ball screw displaces 1 cm as the stepper motor rotates one circle. The elongation of the two-phase stepper motor was 1.8°, one circle containing 200 pulses. Set the starting velocity of the stepper at 60-200 r/min, and the working velocity at 300-600 r/min. The working velocity could reach more than 1000 r/min when unloaded. But high working velocity could lead to a locked-rotor situation. So working velocity should be at a normal level considering the load.
In order to fully perform the velocity of the motor, the motor is usually started at the start frequency and then increases gradually to the pulse frequency until it gets to the ideal velocity. When changing the velocity, we should avoid out-of-step of the motor and try to shorten the starting acceleration. Meanwhile, in order to ensure the positioning accuracy, the velocity of the motor should be reduced from the maximum to the velocity appropriate for being stopped (equals or slightly higher than the starting velocity) by gradually reducing the pulse frequency [22,23] .
Set the start and stop r/min at 100 r/min (or 0.01667 m/s), the maximum r/min at 600 r/min (or 0.1 m/s), and the maximum acceleration at 0.1 m/s 2 . Considering the operational capacity of the actuator, the maximum of the velocity and acceleration was limited to calculate the shortest time.

Analysis of X-axis motion trajectory
The displacement time relation X = G(T) of the X-axis was substituted into the trajectory equation to get the displacement time function Y=Z(T) and the curve of the Y-axis.
X-axis displacement function: as shown in Equation (14). The X-axis displacement time curve is shown in Figure 7. Take the derivative of the X-axis of the trajectory to get the acceleration function, as shown in Equation (16).
The acceleration curve is shown in Figure 9.  Figure 9 Relationship between time and acceleration of X-axis

Analysis of Y-axis motion trajectory
The displacement trajectory was fitted by changing the motor velocity in the vertical direction. In order to acquire the curves of the displacement, velocity and acceleration of Y-axis, the points on the displacement curves of X-axis at the same time points were selected, and the Y-axis values at the corresponding time points to the X-axis were listed according to the trajectory function curve Y = F(X). Then the Y-axis displacement curve was fitted through the curve fitting method.
On the trajectory, the time for one pick-up and dropping circle is 1.8444 s. The X-axis displacement time was divided into 10 equal sections. Take the points every 0.18444 s and get the corresponding X-axis values according to Equation (14). To ensure the system works in a selected range, the curve was moved down 0.0575 units, and the values were substituted into Equation (13) to get the corresponding X-axis and Y-axis values under the equal time intervals, as shown in Table 4. During the trajectory planning, set the coordinate of the pick-up position as (-0.575, 0.1603125). The curve was moved down 0.1603125 units to better show the image, and the data were shown in Table 5.  Y-axis displacement time fitting was performed using MATLAB polynomial curve fitting.
The initial coordinates were the coordinates of the 11 points in Table 5. The fourth order polynomial curve fitting was performed to fit the original data more accurately.
The expression of fitted four order No. 1 trajectory displacement time curve is shown as Equation (17). The fitted Y-axis displacement time image is shown in Figure 10.
The Y-axis velocity function curve is shown in Figure 11.

Figure 11 Relationship between time and speed of the Y-axis
The Y-axis velocity function of the trajectory was derived and the acceleration function was obtained as Equation (19) The Y-axis acceleration function is shown in Figure 12. PLC was applied to control the stepper motor and to track its velocity. The trajectory contained the motion on X-axis and Y-axis. Therefore, we tracked the velocity curves on both directions (X and Y axes) by matching the tracking velocity segment by segment, thus tracking the displacement trajectory [24] .
The velocity curve in the trajectory planning was graded according to velocity; the time was divided into 10 equal sections ( Figure 13). The velocity was adjusted in line with these divided time points; the adjusted velocity was the one at the moment of the rectangle's mid-point. The relationship between the obtained tracking velocity and the curve was shown in Figure 13.
The velocity curve of the trajectory along the X and Y axes was also divided according to the time; the shorter the interval between the equally divided moments, the more the controlled velocity curve resembles the original curve.  Table 6.
The given stepping angle of the stepper motor was 1.8°; one rotation of the stepper motor requires 200 pulses and results in 1 cm linear displacement. The frequency corresponding to the velocity of the No. 1 trajectory could be obtained by Equation (20). The number (D) of pulses was calculated by Equation (21). Table  7 shows the motor control parameters from the pick-up position to the dropping position. Positive numbers of D (the number of pulses) indicate forward rotation (along the positive direction of X and Y axes), while negative numbers indicate reverse rotation (along the negative direction of X and Y axes). 100

Establishment of simulation model
Simulink was used to build the control system model, including the driver module of the stepper motor, the motor module of the two-phase hybrid stepper motor, the screw rod module; the directional signal and the pulse signal were the system input; the angular velocity, torque and mechanical angle were the system output. The output angular velocity was controlled by adjusting the input pulse number, thus tracking the route. The control system model (Figure 14) was established based on parameter settings.   Figure 14 Control model of seedling pick-up arm movement

Simulation of motion trajectory
Based on the control parameters of Table 7, PLC was used to control the drivers of the stepper motors along the X and Y axes [25] . The control parameters included the frequencies of the output pulses and the high low-levels of the direction ports. During the simulation, the control parameters were set to change every 0.18444 s in line with Table 7 to alter the pulse frequency and the direction signal. At first, the X-axis motor was rotating positively with a pulse frequency of 512.84 Hz and a low-level direction signal; the Y-axis motor was also rotating positively with a pulse frequency of 156.38 Hz and low-level direction signal. Change these parameters according to Table 7 every 0.18444 s until all these 10 groups of parameters were used, marking the completion of the trajectory tracking. Figure 15 is the displacement curve.
Totally, there were 10 sections, standing for the simulated curve segments under 10 groups of control parameters.

Analysis of motion trajectory
The simulation curve (the blue curve) matched the planned trajectory curve (the red curve) very well despite some deviations.
The two largest deviations ( Figure 16) were chosen for analyses. At the 0.024 m of the X direction, the deviation along Y direction was 5×10 -4 m; at the 0.038 m of the X direction, the deviation along Y direction was 4×10 -4 m. The standard deviations were 7.46% and 4.65%, which satisfied the control requirements.  The seedlings were picked up and dropped along the trajectory shown in Figure 3; the time used for this process was calculated. From the pick-up position to the dropping position 1, the distances from the bottom to the top and from the right to the left were 5.2 cm and 11.5 cm, respectively. As shown in Figure 17, the torque was vibrating when it started; 0.02 s later, it began sending forth regular outputs at normal angular velocity. Set 0.02 s as the start time, 0.052 m as the up-shift distance, 0.01667 m/s as the start-stop velocity, 0.1 m/s as the maximal velocity, and 0.1 m/s 2 as the maximal acceleration.
When the motor start-up is calculated, make the motor run at the maximal acceleration for 0.57343 s until 0.074013 m/s. Then, slow the acceleration down to its maximum for 0.57343 s until the motor reaches the start-stop velocity. The time used for this step is 0.02+0.57343+0.57343=1.16686 s.  (22), (23) and (24); run the motor at its maximal acceleration for 0.517251 s until it reaches 0.068395 m/s; slow the acceleration down to its maximum for 0.517251 s until the motor reaches the start-stop velocity; run the machine along the route based on the planned trajectory; The time used for this step is 0.02+0.517251+0.517251= 1.054502 s. Taking the 1.8444 s for previous route planning into consideration, the total time required for this process is 2.898902 s. The time spent on the planned route is 0.5339 s less than that on the original one. When the seedling is dropped to the planned position, let the mechanical arm back to the start position for the next picking-up operation. Totally, it will save about 1.0678 s for one circle of pick-up and dropping process.
According to Figure 17, the instantaneous torque could reach 2.5 Nm at the starting moment of the motor; the normal torque is around 1.51 Nm. The sudden increase of torque at the starting moment will result in wobble and vibration of the motor. As shown in Table 1, if we run a circle of seedling pick-up and dropping movement along the original trajectory, there would be 6 start-ups in total by the vertical and horizontal motors, causing motor vibrations. In contrast, the motion based on the planned trajectory only includes one start-up, in which the vertical and horizontal motors will start and stop simultaneously. This can reduce the occurrence of motor vibration and improve the stability of the system. Figure 18 is the simulated angular velocity oscillogram of the motor model under the frequency of 200 Hz; the operating time is set as 0-0.05 s; there is no normal output angular velocity during the 0-0.01 s at the starting moment due to the vibration of the motors; normal output occurs 0.01 s later; the lack of two angular velocities would lead to the phenomenon of out-of-step. During the 0.05 s stop moment, the machine will output three extra angular velocities because of rotational inertia and cause overstep. The start-stops of the motor could cause out-of-step and overstep, as well as displacement errors. The more the start-stops, the larger the displacement errors. The out-of-step and overstep would be more distinct as the motor accelerates. Under the planned trajectory, there is only one start-stop operation for the vertical and horizontal motor during one seedling pick-up and dropping process, which can effectively reduce the displacement errors caused by frequent start-stops. Figure 18 Motor model simulation angular velocity waveform

Trajectory following test
The track-following test was carried out in a fine agriculture laboratory, on April 20, 2019. The 8×16 hole dish was used in the experiment, and the seedlings were taken in the whole row. The test method is to control the actions of horizontal motor and vertical motor respectively by inputting the corresponding pulse number to PLC in each subdivision period, to control the combined motion of the whole row of seedlings, and realize the manipulator to move by the planned trajectory.

Figure 19 Test bench
In order to verify the coincidence degree of the actual movement trajectory of the seedling hand and the planned trajectory, 5 experiments were carried out. The test process is to select five points with the same interval of horizontal coordinates respectively. Whenever the manipulator moves to this point, the longitudinal phase displacement of this point is measured by a 50-degree vernier caliper to obtain the point coordinates. By comparing the measured Y-axis displacement with the planned Y-axis coordinate, the fitting degree between the actual motion trajectory and the planned trajectory is obtained. The size of the error and the fitting degree are inversely proportional.
Main test index: According to the test process, the whole row seedling taking manipulator runs smoothly under the drive of the stepper motor. In the operation process of a cycle, the horizontal motor has a start and stop, the vertical motor has a start and stop, there is no interruption point during the operation process, there is no obvious vibration of the motor. As can be seen from Table 1, the manipulator can follow the planned trajectory at the five points selected at equal intervals. The average value of the five measurements at point 1 and point 5 is closer to the theoretical value, and the relative error is small, only 0.56% and -1.79%, indicating a good tracking effect. The relative errors at point 3 and point 2 are relatively large, 7.54% and 3.85% respectively, which are similar to the simulation results and also within the allowable error range.

Conclusions
(1) In this paper, the whole row of transplanting hands for the transplanter with fixed-point seedling picking and interval delivery are studied. Trajectory planning is made in this paper using smooth circular arc interpolation algorithm. This paper can ensure the simultaneous movement in both horizontal and vertical directions. The motion trajectory is smooth without discontinuity points.
(2) PLC tracking control method is proposed to track the planned trajectory, and a driving system model of the manipulator is established for simulation verification. Results show that the method can save 1.0678 s for each seedling pick-up and dropping process, which can improve the pick-up and dropping efficiency of seedlings.
(3) After trajectory planning, the start-stop times of the motor can be reduced from 6 to 2 to reduce the vibration during the start-stop moment and improve the stability of the system; meanwhile, reduction of start-stop times can lessen the out-of-step and overstep phenomena during the start-stop moment and enhance the positioning accuracy in the seedling pick-up and dropping process.