DESIGN OF A CONTROL SYSTEM FOR A SAFFLOWER PICKING ROBOT AND RESEARCH ON MULTISENSOR FUSION POSITIONING

ABSTRACT This paper discusses the design of a safflower picking robot control system and focuses on a navigation control subsystem based on multisensor fusion. A navigation subsystem, an identification and positioning subsystem, a picking subsystem, and a levelling subsystem are designed. The hardware and software of the navigation subsystem are designed in detail, and a multisensor fusion positioning method based on extended Kalman fusion technology is proposed. The accuracy and stability levels of different combined navigation methods are compared. To test the effectiveness and accuracy of the proposed method, an outdoor test is carried out. The test results show that the outdoor fusion positioning accuracy of the robot is less than 8 cm, and when the satellite signal is lost, the navigation control subsystem can still provide high positioning accuracy. The final positioning result obtained using the integrated positioning method of the wheel odometer + IMU + DGNSS is approximately 52% higher than that of the odometer, approximately 29% higher than that of the wheel odometer + IMU, and approximately 11% higher than that of the IMU + DGNSS.


INTRODUCTION
Safflower, which is a red and blue flower, is a special economic crop that is used for medicinal and oil products (State Pharmacopoeia Committee, 2010).It is difficult to realize mechanized harvesting of safflower due to the characteristics of safflower blooming in multiple batches, different stalk heights and uneven distribution of flower bulbs (Cao et al., 2018;Azimi et al., 2012).Safflower picking robots can not only improve agricultural productivity, reduce operating costs and solve the problem of an inadequate workforce, but also change the agricultural production model to realize the scale, diversification and precision of agricultural production (Wang et al., 2019;Judd & Knasinski, 1990).Therefore, the study of safflower picking robots is important for the automatic harvesting of safflower in fields and upgrading the safflower industry.
One of the key issues for safflower picking robots in picking operations is positioning and navigation.The working environment of safflower picking robots is poor and is characterized by uncertainty and inhomogeneity (Ge et al., 2015;Ichiura et al., 2020).In such an environment, a safflower picking robot wants to achieve precise operation and safe autonomous movement.It must have strong antiinterference and scene perception capabilities, and it is difficult for a single sensor to meet the positioning requirements (Fountas et al., 2020).
At present, the common positioning methods of field robots are the difference global navigation satellite system (DGNSS), inertial measurement units (IMUs) and wheel odometers (Bonadies & Gadsden, 2019).DGNSS can provide all-day, all-weather absolute position and heading information, but the loss of DGNSS signals due to extreme environments or occlusions limits its application in complex farmland environments (Gao et al., 2018;Mahmud et al., 2020).An IMU is a relative parameter solving system with gyroscopes, accelerometers, and geomagnetic field sensors as sensitive elements and does not depend on external information.An IMU can obtain high-frequency relative position and heading information through trajectory estimation, but it cannot address the Engenharia Agrícola, Jaboticabal, v.43, n.2, e20210238, 2023 cumulative error caused by temperature and integration (Alatise & Hancke, 2020).A wheel odometer can obtain the relative position of the vehicle body by monitoring the speed of the driving wheel (Khablov, 2021).It has a low cost and fast response, but the accumulated error is higher due to bumps on the ground and wheel skidding (Youssef et al., 2021).Gao et al. (2022) proposed an improved algorithm for position estimation based on multisensor fusion and autoencoder neural network.The extended Kalman fusion algorithm is used to fuse the positioning data of DGNSSs, IMUs and dual rotary encoders.To achieve improved fusion positioning accuracy, the system combines an autoencoder and radial basis function (ARBF)-based neural network algorithm to optimize the extended Kalman filter.Although this control algorithm can yield significantly improved navigation accuracy, its computational complexity is high, and it requires high processing performance from the navigation controller.This paper does not compare the navigation performance differences observed under different combinations.Jiang et al. (2022) proposed a positioning system with a GNSS, an IMU, and vision fusion that was applied to autonomous vehicles.The extended Kalman fusion algorithm can effectively improve the positioning accuracy of vehicles.
In this paper, aiming at the difficulties of mechanized safflower harvesting and the low positioning accuracy and poor stability of a single sensor, a safflower picking robot control system is designed and built.Combining the characteristics of DGNSS, IMUs and wheel odometers, a fusion localization algorithm based on extended Kalman fusion (EKF) technology is proposed.
The positioning accuracy and stability levels of different combinations, including IMU + DGNSS, wheel odometer + IMU, and wheel odometer + IMU + DGNSS, based on the extended Kalman fusion algorithm are compared.This algorithm provides a feasible solution to the localization problem of safflower picking robots in the field.

HARDWARE DESIGN OF THE CONTROL SYSTEM
Overall design of the control system The structure block diagram of the safflower picking robot, as shown in Fig. 1, consists of five parts: an automatic navigation subsystem, a power subsystem, a safflower recognition and positioning subsystem, a picking subsystem, and a leveling subsystem.The automatic navigation subsystem includes industrial computer terminals, DGNSS mobile stations, IMU sensors, ultrasonic sensors, and servo differential drives.The power subsystem includes a 48 V lithium iron phosphate battery, a power conversion module, and a BMS communication module.The safflower recognition and positioning subsystem includes a binocular stereo vision camera, a safflower recognition and positioning subcontroller, and a camera translation device.The picking subsystem includes a safflower conveying device, safflower shearing device, parallel mechanical arm, and mechanical arm servo drive.The vehicle body leveling subsystem includes an inclination sensor, a cable sensor, and an electric push rod.The robot uses a 48 V lithium battery to power the entire robot and supplies power to different electrical components through a DC power conversion module.The complete machine diagram of the safflower picking robot is shown in Fig. 2. The safflower picking robot operation process is as follows.The robot is powered on, the control system is initialized, and the industrial computer terminal receives the scheduling instructions issued by the scheduling system.The navigation subsystem controls the robot to move to the safflower waiting area according to the scheduling instructions.The leveling subsystem monitors the posture of the picking platform using data from the inclination and cable displacement sensors and controls the electric push rod inside the support legs to raise or lower to adjust the posture of the picking platform.After the leveling of the picking platform is completed, the safflower recognition and positioning subsystem uses the binocular camera to identify and locate a safflower, sends the threedimensional coordinates of the safflower to be picked to the picker controller, and then starts the picking operation.The picking hand controller first controls the action of the parallel mechanical arm and then drives the safflower shearing device to move to the position where the safflower is to be picked.Then, the cutting device is controlled to cut the safflower silk, and the cut safflower silk enters the storage box through the safflower conveying device and is collected and temporarily stored.The safflower picking robot obtains path information for the vehicle body in the field according to the dispatching instructions (Chen et al., 2021).First, the industrial computer terminal obtains the current longitude and latitude information of the robot using the DGNSS and controls the robot to move to the starting position of the prepicking area.Then, according to the fusion information of the DGNSS and IMU, the vehicle body angle and posture information are obtained, the robot posture is adjusted so that the heading direction is consistent with the preset path forward direction, and the robot starts to move.Data fusion of the DGNSS, IMU and wheel odometer are carried out by an industrial computer terminal during the moving process, and real-time positioning information of the robot is calculated.Then, the robot is controlled to drive along the preset path by comparing the location information with the preset path information.The industrial computer terminal sends the position information of the safflower picking robot to the dispatching system for real-time display through wireless AP.
The safflower picking robot monitors obstacles in front of the vehicle using ultrasonic sensors and stops and alarms when the monitoring distance is less than a set value.The industrial computer terminal communicates with the motor driver through the CAN bus and drives the servo motors.The servo motors drive the drive wheels through a reducer and chain drive to perform differential motion to enable the robot to travel straight and steer.The gear ratio of the reducer is 30:1, and the chain drive reduction ratio is 2:1.Table 1 shows the technical parameters of the main components of the navigation system.The DGNSS mobile station is a P3DU vehiclemounted mobile station (Shanghai Huace Navigation Technology Co., Ltd.).The DGNSS base station is a B5UA-CHOWYA station (Shanghai Huace Navigation Technology Co., Ltd.).The industrial computer terminal is an APQ-E6 industrial computer (Chengdu Apqi Technology Co., Ltd.).The IMU is an HWT905 (Shenzhen Witt Intelligent Technology Co., Ltd.).The servo motors are 80LCB075C motors (Beijing Hollysys Electric Co., Ltd.).The motor drivers are LS20530DG drivers (Beijing Hollysys Electric Co., Ltd.).

Design of the safflower recognition and location subsystem
The safflower recognition and positioning subsystem includes a binocular stereo vision camera, safflower recognition location controller, and a camera translation device.Based on the principle of parallax, the binocular camera uses left-eye and right-eye lenses at different positions to obtain two images of the same safflower area to be picked (Yang et al., 2017).The safflower recognition location controller calculates the position deviation between the characteristic points of the safflower corresponding to the image and obtains the three-dimensional coordinate information of the safflower to be picked (Luo et al., 2016).The safflower picking robot straddles 3 rows of safflower for picking operations.Due to the camera's field of view, the positioning accuracy of a safflower at the edge of the field of view cannot meet the picking requirements.Therefore, a camera translation device is added to improve the identification and positioning accuracy of edge safflowers.
The camera translation device includes a stepping motor and a ball screw slide.The binocular camera is installed on the ball screw slide.The safflower recognition positioning controller controls the translation of the ball screw slide by controlling the stepping motor, controlling the translation of the binocular camera.

Picking subsystem design
The schematic diagram of the picking subsystem is shown in Fig. 3.The picking subsystem includes a picking controller, a safflower conveying device, a safflower shearing device, a parallel mechanical arm, and a mechanical arm servo drive.The safflower picking system uses a parallel robotic arm to drive an end effector (safflower shearing device) to achieve three-dimensional movement.The parallel robotic arm includes three active arms and three slave arms (Yang et al., 2021).The picking controller obtains the three-dimensional coordinates of the safflower to be picked from the safflower recognition positioning controller and calculates the rotation angle of each servo axis through the inverse kinematics calculation method (Yin et al., 2018).The servo axis drives the active arm, which in turn drives the slave arm.The three slave arms are hinged with the end effector to realize the three-dimensional movement of the safflower shearing device.

Design of the picking platform leveling subsystem
The leveling system of the picking platform is shown in Fig. 5.The levelling subsystem of the picking platform includes an inclination sensor, rope displacement sensor, and linear actuator.Parallel robotic arms and binocular cameras are installed on the picking platform.Due to the uneven ground in the field, it is easy to cause the picking platform to tilt, and the safflower picking robot cannot effectively locate and pick safflowers (Baeten et al., 2008;Li et al., 2016).Therefore, it is necessary to use a leveling subsystem to adjust the picking platform in real time to maintain the level of the picking platform and the area to be picked and improve the accuracy of safflower positioning and picking.
The vehicle body levelling system monitors the inclination angle of the platform relative to the platform center point through an inclination sensor installed in the middle of the picking platform.The controller controls a linear actuator (Fig. 1 (8)) on the inner sides of the 4 supporting legs to rise or fall according to the inclination information and then adjusts the position of the picking platform.The controller monitors the elongation of the linear actuator through the rope displacement sensor to prevent the linear actuator from overranging during adjustment.

Navigation path planning
After the safflower picking robot is powered on and initialized, it waits for the dispatch terminal to send the coordinates of the online point and the heading to that point.The robot controls the differential wheels of the vehicle to travel to the online point according to the instructions from the dispatch terminal.After adjusting the posture, the robot waits again for the dispatch terminal to send the movement task chain.
For the Chinese safflower mechanized planting mode, the most commonly used methods are wide and narrow row planting.Wide row spacing is 450 mm and narrow row spacing is 200 mm; the plant height is 450-750 mm (Jiao, 2019).As shown in Fig. 6, the trajectory line with arrows represents the preset navigation path of the safflower picking robot, point 0 is the online point, and points 1-9 are task chain nodes (turning points).FIGURE 6. Field movement task chain of the safflower picking robot.

Positioning and navigation frame of the safflower picking robot
The process framework of multisensor fusion positioning and navigation is shown in Fig. 7. Traditional Kalman filtering is only suitable for systems that are linear and satisfy Gaussian distribution, but the field navigation environment of the safflower picking robot is not linear (Zhang et al., 2021;Silva & Cruz, 2016).Aiming at the complex navigation environment in the field where the safflower picking robot is located, this paper adopts the EKF algorithm to realize the data fusion of the navigation sensors.This paper uses the EKF algorithm to fuse the positioning information provided by the wheel odometer, IMU, and DGNSS differential navigation module.The EKF consists of two parts: a prediction step and an update step.First, the positioning information (DOdo, X, DOdo, Y, θOdo) provided by the wheel odometer is used as the prediction step.The position information provided by the DGNSS (XGNSS, YGNSS, θGNSS) is used as the update step observation.Second, the fused wheel odometer and DGNSS positioning information is used as the prediction step, and the heading information θIMU provided by the IMU is used as the update step to perform data fusion again.The localization results (XEKF, YEKF, θEKF) are output by the EKF multisensor fusion algorithm.
During safflower picking robot navigation, the difference between the positioning information (DOdo, X, DOdo, Y, θOdo) and the target path (XT, YT, θT) is used as input to a fuzzy PID controller.The output is the rotational velocity values (Vl, Vr) of the left and right driving wheels to realize the path tracking and heading adjustment of the safflower picking robot.

EKF multisensor fusion algorithm
It is assumed that the working area of the picking robot is an ideal horizontal two-dimensional environment, and the state vector of the system is the position and heading of the robot.The position and heading of the picking robot at time k is known to be xk = [Xk, Yk, θk] T , where Xk and Yk denote the position of the geometric center of the picking robot and θk represents the heading of the picking robot.

State prediction
According to the EKF algorithm, once the motion model of the mobile robot is accurately provided, the position and heading at time k+1 can be expressed as Engenharia Agrícola, Jaboticabal, v.43, n.2, e20210238, 2023 Where: is the state variable at k+1;  is the control quantity at k+1; ( ,  ) is the functional relation between the state variable  and the state variable  ,  is the motion noise at time k+1.
Here, the wheel odometer is used to provide the control amount  = [DOdo,X, DOdo,Y, θOdo] for the prediction step.
(1) Prediction of the position and heading at time k+1 based on the motion model of the picking robot. Where: is the position and heading estimation value at time k+1 predicted by the motion model, where the symbol "^" is the estimated value and the symbol "-" is the predicted value.
(2) The covariance matrix of the prior estimate of the predicted state vector is Where: Pk and  are the covariance matrices of the position and heading  and the motion noise  , respectively.
∇ and ∇ are the Jacobian matrix of the motion model and the Jacobian matrix of the motion noise, respectively.

Measurement update
Using the position information provided by the DGNSS [Xk+1, GNSS, Yk+1, GNSS, θk+1, GNSS], the observation model at this time is Where:  is the system observation value at time k+1, and  is the observation noise at time k+1.

𝐾 = 𝑃 ∇ℎ ∇ℎ 𝑃 ∇ℎ + 𝑅
-1 (5) Where: ∇ℎ is the Jacobian matrix of the measurement model, and it is known from the measurement model that ∇ℎ is a unit matrix. is the covariance matrix of the observation model.Generally, the accuracy is given by the manufacturer, or a noise evaluation is carried out using experimental statistics.
(2) Determine the posterior of the state variables.
(3) Update the covariance matrix to the covariance matrix of the posterior estimated value of the state variable.
In the formula,  is the third-order identity matrix.
The data fusion result  , of the wheel odometer and DGNSS is used as the data for the prediction step, and the heading result θIMU obtained by the IMU is used as the observation for the update step.The above data fusion process is repeated to obtain  , = [ ,  ,  ].

Test evaluation
To analyze and verify the accuracy and stability of the designed safflower picking robot fusion positioning system, a positioning accuracy test was carried out. Where: δ is the positioning accuracy, m; Zi is the coordinate of the point measured in the trajectories of the different positioning methods, Z0 is the coordinate of the point measured in the reference trajectory.
The outdoor fusion positioning accuracy of the safflower picking robot is less than 8 cm, and when the satellite signal is lost, the control system can still provide high positioning accuracy.The final positioning result obtained by the multisensor fusion-based positioning method is approximately 52% higher than that of the odometer positioning method, approximately 29% higher than that of the wheel odometer + IMU, and approximately 11% higher than that of the IMU + DGNSS.This shows that the multisensor fusion positioning method proposed in this paper achieves high positioning accuracy and stability.

CONCLUSIONS
This paper proposes a safflower picking robot control system and completes the overall design of the system and the design of each subsystem.The hardware and software of the automatic navigation system of the safflower picking robot were designed and built in detail.A movement path for the safflower picking robot in a field was planned, and a fusion positioning algorithm of a wheel odometer + DGNSS + IMU based on extended Kalman fusion was proposed.The positioning accuracy and stability levels of different positioning combinations based on the extended Kalman fusion algorithm were compared.
To evaluate the feasibility and positioning accuracy of the proposed positioning method, a fusion positioning test of the safflower picking machine was conducted.The test results show that the outdoor fusion positioning accuracy of the robot is less than 8 cm, and when the satellite signal is lost, the control system can still provide high positioning accuracy.The final positioning results obtained by the multisensor fusion positioning method are improved by approximately 52% compared with the odometer positioning accuracy, by approximately 29% compared with the wheel odometer + IMU fusion accuracy, and by approximately 11% compared with the IMU + DGNSS accuracy.Therefore, multisensor fusion helps the safflower picking robot obtain more accurate positioning and better positioning stability and lays the foundation for the field navigation of the safflower picking robot.

FIGURE 1 .
FIGURE 1. Block diagram of the safflower picking robot.
FIGURE 2. Structure and picture of the safflower picking robot Design of each subsystem Navigation subsystem design

FIGURE 3 .
FIGURE 3. Schematic diagram of the picking subsystem.The schematic diagram of the safflower shearing device is shown in Fig. 4. The safflower shearing device includes a flower ball guide groove and a crank slider shearing mechanism.The safflower cutting device moves above the safflower to be picked with the parallel manipulator and moves slowly downward.The safflower enters the safflower cutting device through the ball guide groove, and the safflower filament is cut by the action of the crank slider shearing mechanism.The cut safflower

FIGURE 7 .
FIGURE 7. Flow diagram of the fusion of the positioning navigation.
is the variance of the observation noise output by the DGNSS with respect to position X;  is the variance of the observation noise output by the DGNSS with respect to position Y, and  is the variance of observed noise about position and heading output by the DGNSS.
FIGURE 8. Outdoor test site.Positioning accuracy testThe movement area selected was a 5 m×5 m square plot, and the vertices of this square area were defined as o, a, b, and c.The picking robot was placed in this area, and the manual control mode was set.The picking robot was placed at the coordinate origin o (0,0) m, 3 target points a (5,0) m, b (5,5) m, c (0,5) m were set for the picking robot, and the vehicle speed was set to 0.2 m/s.The picking robot passed through coordinate points o, a, b, and c in turn

Fig. 9
Fig.9shows that the curve of the wheel odometer positioning trajectory track drifts compared to the reference trajectory.The curve of the DGNSS positioning trajectory fluctuates around the positioning reference trajectory, indicating that there is extensive noise in the DGNSS positioning, but there is no drift.The DGNSS positioning accuracy is susceptible to occlusion by obscurants.Compared with the wheel odometer, the positioning trajectory curve of the wheel odometer + IMU fusion is closer to the reference trajectory.The wheel odometer + IMU + DGNSS fusion positioning trajectory curve is closer to the reference trajectory, and compared

TABLE 1 .
Main device technical parameters.