Multi-sensor fusion blueberry greenhouse inspection vehicle control system and control method thereof

Abstract

The invention belongs to the field of agricultural machinery, and discloses a multi-sensor fusion blueberry greenhouse inspection vehicle control system and a control method thereof, the control system is integrated in an inspection vehicle, and the control system is mainly composed of a core control module, a chassis power module and an environment sensing module. The environment sensing module is responsible for acquiring environment information of a blueberry greenhouse operation area where the inspection vehicle is located, constructing an environment map, detecting surrounding obstacles and achieving real-time obstacle avoidance. The core control module generates a path and a control instruction according to sensor data and transmits control information to the chassis power module. And the chassis power module drives the inspection vehicle to move according to the received control information. The system adopts a multi-sensor information fusion algorithm, ensures accurate positioning and high-stability navigation performance, and can be widely applied to autonomous inspection operation of the blueberry greenhouse.

Description

Technical Field

[0001] The present invention belongs to the field of agricultural machinery, and particularly relates to a control system and a control method for a blueberry greenhouse inspection vehicle with multi-sensor fusion. Background Art

[0002] With the development of China's facility agriculture, large-scale blueberry greenhouses have gradually increased, and the inspection operations in blueberry greenhouses have become increasingly important. Traditional inspection operations in blueberry greenhouses mainly rely on manual operation, which not only has a large labor intensity but also has low efficiency, making it difficult to meet the inspection needs of large-scale blueberry greenhouses.

[0003] Currently, the control range of blueberry greenhouse inspection vehicles on the market is limited, lacking route planning for the operation area, unable to fully inspect blueberry greenhouses, and unable to avoid obstacles autonomously. In addition, the sensors carried by current inspection vehicles on the market are relatively single, unable to accurately identify and locate obstacles, and the obstacle avoidance algorithm is not precise enough to avoid dynamic obstacles, resulting in low safety performance. Summary of the Invention

[0004] To solve the problems existing in the prior art, the present invention provides a control system and a control method for a blueberry greenhouse inspection vehicle with multi-sensor fusion, which can navigate autonomously in a blueberry greenhouse, realize the inspection operation of the blueberry greenhouse area, and can avoid obstacles in real time, with good stability and safety, reducing labor costs while improving the efficiency of blueberry greenhouse inspection operations.

[0005] To achieve the above object, the present invention provides the following solutions:

[0006] A control system for a blueberry greenhouse inspection vehicle with multi-sensor fusion, the system includes: a power supply module, a core control module, a chassis power module, an environment perception module, and a remote control module;

[0007] The power supply module is used to provide the required power for the system;

[0008] The environment perception module is used to obtain the environmental information of the blueberry greenhouse operation area where the inspection vehicle is located, construct an environmental map, detect surrounding obstacles and realize real-time obstacle avoidance;

[0009] The core control module is used to generate a path and control instructions according to the sensor data of the environment perception module, and transmit the control information to the chassis power module;

[0010] The chassis power module is used to drive the inspection vehicle to move according to the received control information;

[0011] The remote control module is used for the operator to manually control the inspection vehicle through a remote controller.

[0012] Preferably, the core control module consists of an industrial control computer, a single-chip microcomputer, and a remote control system;

[0013] The industrial control computer is connected to the single-chip microcomputer through the CAN bus to achieve information exchange; in the same local area network environment, the operator remotely logs in to the industrial control computer through the SSH protocol to start the robot operating system, thereby realizing the distributed remote control of the system.

[0014] Preferably, the chassis power module includes a motor drive module, a motor, and a traveling track;

[0015] The motor drive module is connected to the pins of the motor through the motor interface, and the drive signal pin of the motor drive module is connected to the GPIO pin of the single-chip microcomputer. The motor is connected to the traveling track through a mechanical structure to drive the track to realize the movement of the inspection vehicle.

[0016] Preferably, the environment perception module includes a lidar, a depth camera, and an IMU sensor;

[0017] The lidar and the depth camera are installed at the exact middle position in the front of the inspection vehicle, and the IMU sensor is installed at the center of the inspection vehicle and aligned with the symmetry axis of the inspection vehicle;

[0018] The lidar, the depth camera, and the IMU sensor jointly transmit data to the industrial control computer, and the industrial control computer can perceive the greenhouse environment and collect environmental point cloud information; the IMU sensor is responsible for detecting the pose of the inspection vehicle and making adjustments; the industrial control computer processes the information using multi-sensor fusion algorithms and path planning and obstacle avoidance decision algorithms, generates a greenhouse environment map, adjusts the pose of the inspection vehicle, and generates control information; finally, the industrial control computer transmits the control information to the motor drive module through the single-chip microcomputer to realize that the inspection vehicle performs the inspection task according to the predetermined path and performs real-time obstacle avoidance.

[0019] Preferably, the remote control module consists of a model airplane remote control and a signal receiver;

[0020] The signal receiver is connected to the single-chip microcomputer and is responsible for information transmission. The model airplane remote control communicates with the signal receiver through a wireless signal, and the operator manually controls the inspection vehicle through the remote control.

[0021] Preferably, the power supply module includes an inspection vehicle battery, a boost module, and a voltage stabilization module;

[0022] The inspection vehicle battery supplies power to the motor drive module to drive the motor to operate; the boost module raises the voltage of the inspection vehicle battery and supplies power to the industrial control computer through a power cord; the voltage stabilization module stabilizes the battery voltage and provides a stable power supply to the single-chip microcomputer.

[0023] The present invention also provides a control method for a blueberry greenhouse inspection vehicle with multi-sensor fusion, which is characterized in that it is implemented by using the multi-sensor fusion control system for a blueberry greenhouse inspection vehicle described in any one of the above, and the method includes the following steps:

[0024] S1. Initialize the hardware of the inspection vehicle control system;

[0025] S2. Operate the inspection vehicle to move in the greenhouse rows through the remote control module, collect point cloud data, and complete the data fusion of the depth camera and the lidar;

[0026] S3. Generate an environmental map based on the fused data by using the cartographer slam algorithm;

[0027] S4. Generate a path for the inspection operation based on the environmental map;

[0028] S5. Drive the inspection vehicle to perform greenhouse inspection operations and autonomously avoid obstacles in real time according to the environmental perception module and the chassis walking module to complete the operation.

[0029] Preferably, in the step S2, the method of operating the inspection vehicle to move in the greenhouse rows through the remote control module, collecting point cloud data, and completing the data fusion of the depth camera and the lidar includes:

[0030] S201. Complete the joint calibration of the lidar and the depth camera, obtain the spatial conversion relationship between the lidar and the depth camera, and output a parameter file of the joint calibration;

[0031] S202. Use the lidar to obtain the surrounding environment data and transmit it to the industrial control computer, and calculate the distance between the inspection vehicle and the obstacle;

[0032] S203. Use the depth camera to obtain the surrounding environment data and transmit it to the industrial control computer, identify the type of obstacle and calculate the depth information of the obstacle;

[0033] S204. Use the depth camera to convert the depth information into laser-like data and fuse it with the lidar data.

[0034] Preferably, in the step S4, the method of generating a path for the inspection operation based on the environmental map includes:

[0035] S401. The user operates the remote control terminal to remotely run the path planning launch file and enter the path planning operation mode;

[0036] S402. Read the environmental model map, access the data coupled by the lidar and the depth camera for positioning, and access the IMU sensor data to adjust the pose of the inspection vehicle;

[0037] S403. Use the A* algorithm for path planning;

[0038] S404. Generate a rasterized environmental model map and save the map;

[0039] S405. End the process of the mapping launch file and exit the environmental map building mode.

[0040] Preferably, in step S5, according to the environmental perception module and the chassis walking module, driving the inspection vehicle to perform greenhouse inspection operations and autonomous real-time obstacle avoidance, the method for completing the operations includes:

[0041] S501. The user remotely starts the autonomous obstacle avoidance launch file through the remote control terminal to enter the autonomous obstacle avoidance mode;

[0042] S502. Read the rasterized environmental model map, receive the data coupled by the lidar and the depth camera, perform multi-sensor fusion positioning, and complete the positioning of the inspection vehicle of the lidar, the depth camera, and the IMU sensor;

[0043] S503. The depth camera uses the YOLOv5 network to identify the types of obstacles, calculates the depth information of the target obstacles, and combines the obstacle distance information provided by the lidar to improve the positioning accuracy;

[0044] S504. When the inspection vehicle encounters an obstacle, enable the local path planning DWA algorithm to avoid the obstacle and resume to the original operation path;

[0045] S505. When the inspection vehicle completes the inspection operation in the blueberry greenhouse, stop at the operation end point and end the inspection task.

[0046] Compared with the prior art, the beneficial effects of the present invention are:

[0047] 1) In the multi-sensor fusion blueberry greenhouse inspection vehicle control system provided by the present invention, by perceiving the environmental information of the target blueberry greenhouse, the core control module constructs an operation map of the inspection vehicle in the operation area of the target blueberry greenhouse and accurately completes the positioning of the inspection vehicle in the target blueberry greenhouse;

[0048] 2) In the multi-sensor fusion blueberry greenhouse inspection vehicle control method provided by the present invention, an environmental model map is established through the cartographer slam algorithm, with accurate positioning, marking the impassable areas, and improving the passing rate of the inspection vehicle.

[0049] 3) The control method of a multi-sensor fusion blueberry greenhouse inspection vehicle provided by the present invention is based on the established environmental map and uses the A* algorithm to perform greenhouse inspection operations, safely and efficiently realizing the inspection of blueberry greenhouses;

[0050] 4) The control method of a multi-sensor fusion blueberry greenhouse inspection vehicle provided by the present invention fully combines the actual environment of the blueberry greenhouse and flexibly adjusts the forward direction and forward attitude of the inspection vehicle in real time;

[0051] 5) The control method provided by the present invention adopts multi-sensor information coupling, with accurate positioning and high obstacle avoidance stability, and can be widely applied to autonomous inspection operations in blueberry greenhouses. Description of the Drawings

[0052] In order to more clearly illustrate the technical solutions of the present invention, the following briefly introduces the drawings required for use in the embodiments. Obviously, the drawings in the following description are only some embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.

[0053] Figure 1 is a structural block diagram of a multi-sensor fusion blueberry greenhouse inspection vehicle control system provided by an embodiment of the present invention;

[0054] Figure 2 is an overall step flow chart of a control method of a multi-sensor fusion blueberry greenhouse inspection vehicle provided by an embodiment of the present invention;

[0055] Figure 3 is a flow schematic diagram of a control method of a multi-sensor fusion blueberry greenhouse inspection vehicle provided by an embodiment of the present invention;

[0056] Figure 4 is a relationship diagram of a control method of a multi-sensor fusion blueberry greenhouse inspection vehicle provided by an embodiment of the present invention.

[0057] Figure 5 is an A* algorithm flow chart provided by an embodiment of the present invention.

[0058] In the figure:

[0059] 1. Remote control module; 11. Signal receiver; 12. RC transmitter; 2. Core control module; 21. Industrial computer; 22. Microcontroller; 23. Remote control system; 4. Chassis power module; 41. Motor drive module; 42. Motor; 43. Driving track; 7. Environment perception module; 71. LiDAR; 72. Depth camera; 73. IMU sensor; 9. Power module; 91. Inspection vehicle battery; 92. Boost module; 93. Voltage regulator module. Detailed Embodiments

[0060] The following will clearly and completely describe the technical solutions in the embodiments of the present invention with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0061] To make the above objects, features, and advantages of the present invention more obvious and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0062] Embodiment 1

[0063] A control system for a blueberry greenhouse inspection vehicle with multi-sensor fusion provided by the present invention is built inside the inspection vehicle. The control system collects environmental information of the target blueberry greenhouse for map construction, positioning, and controls the inspection vehicle to complete real-time obstacle avoidance. For the control system of the blueberry greenhouse inspection vehicle with multi-sensor fusion provided, a control method for the blueberry greenhouse inspection vehicle with multi-sensor fusion combines multi-sensor fusion positioning and navigation decision algorithms, controls the inspection vehicle to be able to perform autonomous inspection operations in the target blueberry greenhouse, and realizes autonomous navigation, and has good navigation stability, effectively improving the operation efficiency of the inspection vehicle while reducing the amount of manual operation.

[0064] Figure 1 The block diagram of a control system for a blueberry greenhouse inspection vehicle with multi-sensor fusion provided according to an embodiment of the present invention is shown;

[0065] As Figure 1 shown, a control system for a blueberry greenhouse inspection vehicle with multi-sensor fusion provided by an embodiment of the present invention includes: a power supply module 9, and the power supply module 9 provides power for the control system of the self-propelled multi-sensor full-coverage inspection vehicle in the blueberry greenhouse; it also includes a core control module 2, a chassis power module 4, an environmental perception module 7, and a remote control module 1. The core control module 2 includes an industrial computer 21, a single-chip microcomputer 22, and a remote control system 23; the industrial computer 21 is connected to the single-chip microcomputer 22 through a CAN bus to achieve information exchange, and the operator can remotely log in to the industrial computer 21 through the SSH protocol of the remote control system 23 to start the robot operating system and realize distributed remote control of the system.

[0066] The chassis power module 4 includes a motor drive module 41, a motor 42, and a drive track 43; the motor drive module 41 is connected to the pins of the motor 42 through an interface, and the drive signal pin of the motor drive module 41 is connected to the GPIO pin of the single-chip microcomputer 22, and the motor 42 is mechanically connected to the drive track 43.

[0067] The environmental perception module 7 includes a lidar 71, a depth camera 72, and an IMU sensor 73 (inertial navigation sensor); the lidar 71 and the depth camera 72 are installed at the center front position of the inspection vehicle. The lidar 71, the depth camera 72, and the IMU sensor 73 are connected to the industrial computer 21 to identify obstacles and boundaries in the blueberry greenhouse and avoid obstacles. The IMU sensor 73 is installed at the center position of the inspection vehicle and aligned with the symmetry axis of the inspection vehicle, and is used to detect and adjust the pose of the inspection vehicle. The lidar 71, the depth camera 72, and the IMU sensor 73 jointly transmit information to the industrial computer 21. The industrial computer 21 processes the information through multi-sensor fusion and path planning and obstacle avoidance decision algorithms, calculates the types and positions of obstacles, adjusts the pose of the inspection vehicle, generates control information for the operation area, and transmits the control information to the motor drive module 41 through the single-chip microcomputer 22, and then controls the inspection vehicle to perform full coverage operation in the blueberry greenhouse according to the control information and realize real-time obstacle avoidance.

[0068] The remote control module 1 includes a radio control transmitter 12 and a signal receiver 11; the signal receiver 11 is connected to the single-chip microcomputer 22 to realize information transmission, and the signal receiver 11 communicates with the radio control transmitter 12 to realize signal transmission.

[0069] Among them, the power supply module 9 includes an inspection vehicle battery 91, a boost module 92, and a voltage stabilization module 93;

[0070] Among them, the inspection vehicle battery 91 is connected to the motor drive module 41, and the inspection vehicle battery 91 supplies power for the operation of the motor 42. The boost module 92 is connected to the inspection vehicle battery 91 and supplies power to the industrial computer 21 through a power cord after boosting the voltage; the voltage stabilization module 93 is connected to the inspection vehicle battery 91 and supplies power to the single-chip microcomputer 22 after stabilizing the voltage.

[0071] In an embodiment of the present invention, the industrial computer 21 is selected as an Advantech AR-1500 type computer, which is one of the core control components of a multi-sensor fusion blueberry greenhouse inspection vehicle control system, and is responsible for realizing functions such as processing and fusion of various sensor data, construction of maps, issuing commands for chassis control, processing of task information before navigation, path planning during navigation, and calculating and displaying the running path of the inspection vehicle on a remote control terminal, etc.;

[0072] The single-chip microcomputer 22 is selected as an STM32F407ZGT6 single-chip microcomputer, which is one of the core control components of a multi-sensor fusion blueberry greenhouse inspection vehicle control system, and is responsible for controlling the movement of the chassis power module 4 and calculating functions such as the moving speed and steering information of the inspection vehicle;

[0073] The selection of the specific model of the remote control system 23 is relatively flexible, as long as it runs the Ubuntu system with the robot operating system. The remote control system 23 is responsible for visualizing the data processed in the industrial control computer 21 to facilitate user monitoring and the issuance of remote control instructions.

[0074] The motor drive module 41 selects two DC reduction motor drive boards to drive a DC reduction motor respectively, so as to drive the crawler to move.

[0075] The lidar 71 selects a sixteen-line lidar to identify the boundary of the target operation area and construct a map, and the accuracy of map building and positioning is relatively high.

[0076] The IMU sensor 73 selects a nine-axis IMU inertial navigation module, including a three-axis gyroscope, a three-axis accelerometer, and a three-axis magnetometer, to obtain data such as the yaw angle and pitch angle of the inspection vehicle.

[0077] The depth camera 72 selects the Orbbec Gemini binocular depth camera to identify obstacles and calculate the depth information of the obstacles, with relatively high accuracy.

[0078] The inspection vehicle battery 91 selects two 12V rechargeable lead-acid batteries in parallel to directly supply power to the chassis power module 4. The boost module 92 selects a 12V to 19V sine wave inverter to supply power to the industrial control computer 21, lidar 71, depth camera 72, and IMU sensor 73 through a power cord. The voltage stabilization module selects a 12V to 5V DC power converter to supply power to the single-chip microcomputer 22.

[0079] Embodiment 2

[0080] As Figure 2 shown, the present invention also provides a control method for a multi-sensor fusion blueberry greenhouse inspection vehicle control system, which specifically includes the following steps:

[0081] S1. Initialize the hardware of the inspection vehicle control system;

[0082] S2. The remote control module 1 operates the inspection vehicle to walk in the greenhouse rows, collect point cloud data, and complete the data fusion of the depth camera 72 and the lidar 71;

[0083] S3. Generate an environmental map based on the cartographer slam algorithm;

[0084] S4. Generate the path of the inspection operation based on the environmental map;

[0085] S5. According to the environmental perception module 7 and the chassis walking module, drive the inspection vehicle to perform greenhouse inspection operations and autonomous real-time obstacle avoidance to complete the operation.

[0086] Further, step S1 specifically includes:

[0087] S101. Start the power supply module 9;

[0088] S102. The battery of the inspection vehicle supplies power to the chassis power module 4. After the boost module 92 boosts the voltage of the inspection vehicle battery 91, it supplies power to the industrial control computer 21, and the industrial control computer 21 starts and completes initialization. The voltage stabilization module 93 stabilizes the voltage of the inspection vehicle battery 91 and then supplies power to the single-chip microcomputer 22, and the single-chip microcomputer 22 starts and completes initialization;

[0089] S103. The industrial control computer 21 supplies power to the lidar 71, the depth camera 72, and the IMU sensor 73 through USB connection, and starts and completes initialization with the lidar 71, the depth camera 72, and the IMU sensor 73;

[0090] S104. The industrial control computer 21 and the single-chip microcomputer 22 are connected through the CAN bus and start communicating;

[0091] S105. In the same local area network environment, the operator remotely logs in to the industrial control computer 21 through the SSH protocol and starts the robot operating system to achieve distributed remote control of the system;

[0092] Further, step 2 specifically includes:

[0093] S201. Complete the joint calibration of the lidar 71 and the depth camera 72 to obtain the spatial transformation relationship between the lidar 71 and the depth camera 72, and output the parameter data of the joint calibration;

[0094] Specifically, multi-sensor spatial synchronization is the basis for fusing data from various sensors. In the present invention, the RGB-D camera and the 2D LiDAR are jointly calibrated to obtain the internal parameters and external parameters of the camera, establish the transformation relationship between the radar data coordinates and the camera data coordinates, and achieve this by extracting the corner points in the checkerboard. The camera and the radar collect data from multiple poses, and the spatial transformation matrix between the camera and the radar can be calculated, including the rotation matrix and the translation vector, so as to determine the positional relationship between the camera and the radar in space.

[0095] Specifically, assume that a point q in space is observed by the 2D Lidar and the RGB-D camera, and its coordinates in the laser coordinate system are (X l , Y l , Z l ), and its coordinates in the camera coordinate system are (X c , Y c , Z c ), and the joint calibration model is:

[0096]

[0097] Among them, P c represents the camera coordinate system, and P l represents the lidar coordinate system. R and T respectively represent the rotation matrix and translation matrix between the camera coordinate system and the lidar coordinate system, which can realize the conversion between the two coordinate systems.

[0098] S202. The lidar 71 acquires the surrounding environment information and transmits it to the industrial control computer 21 to calculate the distance between the inspection vehicle and the obstacle.

[0099] S203. The depth camera 72 acquires the surrounding environment information and transmits it to the industrial control computer 21. The YOLO deep learning algorithm is used to identify the type of the obstacle, and combined with the depth map, the depth information of the obstacle recognition frame is calculated.

[0100] S204. The depth information of the depth camera 72 is converted into pseudo-lidar data and fused with the data of the lidar 71.

[0101] To overcome the limitation of 2D LiDAR in detecting obstacles with high consistency in a 3D environment, an RGB-D depth camera can be combined with 2D LiDAR to improve the detection effect in a real environment. The RGB-D depth camera first acquires an RGB image and a depth 3D point cloud. Since the lidar obtains two-dimensional point cloud information and the camera obtains three-dimensional point cloud information, the point cloud data obtained by the two sensors cannot be directly fused. Before fusion, the three-dimensional point cloud projection of the camera needs to be converted into pseudo-lidar point cloud data. Let the spatial point corresponding to a certain pixel coordinate point P(x, y, z) in the depth map be P c (X c , Y c , Z c ). In the figure, θ is the angle between the z-axis direction and the projection point.

[0102]

[0103] Among them, f x is the focal length of the camera, and c x is the principal point of the camera, which are the internal parameters of the camera and are obtained by camera calibration.

[0104] The distance r C from point B to O is:

[0105]

[0106] Among them, d is the original depth value of the corresponding pixel in the depth image.

[0107] The conversion of this spatial point into pseudo-lidar point cloud data can be expressed as (r c , θ c), in a scene without obstacles, the algorithm regards the ground area as a potential obstacle and adds geometric constraints based on the height relationship between the camera and the ground to filter the valid area. Then, the corresponding point of each pixel in the spatial point cloud is obtained and projected onto a two-dimensional plane, and finally, smooth LiDAR-like data is generated.

[0108] In addition, by converting the two-dimensional matrix data of the RGB-D depth camera into two-dimensional row vector data from LiDAR, the information dimension can be effectively reduced, where the minimum value of each column theoretically represents the distance between the nearest obstacle and the camera.

[0109] r i = min(r i1 , r i2 ,..., r ij )

[0110] In the formula, r i represents the distance from the nearest obstacle in the i-th column to the camera, j represents the number of rows of the three-dimensional point cloud, and r ij represents the distance from the obstacle in the i-th column and j-th row to the camera.

[0111] The point cloud data of the LiDAR and the camera are obtained in their respective coordinate systems. Therefore, the data of the LiDAR point cloud is converted to the camera coordinate system through the following formula. After conversion, the point cloud data P i can be expressed as:

[0112]

[0113] where (X i , Y i , Z i ) represents the obstacle coordinates obtained by the LiDAR.

[0114] Furthermore, step S3 specifically includes:

[0115] S301. The user operates the remote control terminal to remotely run the map building launch file and enter the remote control mode;

[0116] S302. The user operates the UAV remote controller 12 to drive the inspection vehicle to move in the blueberry greenhouse to fully collect point cloud information

[0117] S303. Use the cartographer slam algorithm to generate a two-dimensional grid map of the greenhouse environment;

[0118] S304. Exit the map building launch file.

[0119] Furthermore, the said step S4 specifically includes:

[0120] S401. The user operates the remote control terminal to remotely run the path planning launch file and enters the path planning operation mode;

[0121] S402. Read the environmental model map, access the coupled data of the lidar 71 and the depth camera 72 for positioning, and access the data of the IMU sensor 73. The information of each sensor is fused through the extended Kalman filter, and the pose of the inspection vehicle is adjusted through the output fused positioning information;

[0122] S403. The path planning uses the A* algorithm. The A* algorithm is a heuristic search algorithm used to find the shortest path from the starting point to the ending point in a graph. It combines the advantages of the Dijkstra algorithm and the best-first search. By considering both the actual cost from the starting point to the current node and the estimated cost from the current node to the ending point during the search process, the search efficiency is improved;

[0123] S404. Generate a rasterized environmental model map and save the map;

[0124] S405. End the process of the mapping launch file and exit the environmental map building mode.

[0125] Furthermore, step 5 specifically includes:

[0126] S501. The user operates the remote control terminal to remotely run the autonomous obstacle avoidance launch file and enters the autonomous

[0127] obstacle avoidance mode;

[0128] S502. Read the environmental model map, access the coupled data of the lidar 71 and the depth camera 72, and perform multi-sensor fusion positioning of the inspection vehicle for the lidar 71, the depth camera 72, and the IMU sensor;

[0129] S503. The depth camera 72 uses the YOLOv5 network to detect the types of obstacles, calculates the depth information of the target obstacle, and then couples it with the distance information of the obstacle measured by the lidar 71 to improve the positioning accuracy.

[0130] S504. When the inspection vehicle encounters an obstacle, the local path planning DWA algorithm is enabled to accurately avoid the obstacle and return to the original operation path;

[0131] S505. When the inspection vehicle completes the inspection operation in the blueberry greenhouse, the inspection vehicle stops at the operation end point and the inspection operation ends.

[0132] In this embodiment, the launch file is a program startup file written by the developer, rather than generated through the previous steps. Running the autonomous obstacle avoidance launch file starts the autonomous obstacle avoidance mode, running the path planning launch file starts the path planning mode, and running the mapping launch file enters the map building mode.

[0133] Figure 3 It is a schematic diagram of the cartographer slam mapping algorithm for a multi-sensor fusion blueberry greenhouse inspection vehicle provided by an embodiment of the present invention.

[0134] As Figure 3 , cartographer slam is mainly divided into two parts: local SLAM and global SLAM. Local SLAM relies on the fused data of 2D LiDAR and RGB-D cameras to generate multiple Submaps; while global SLAM integrates all submaps into a complete global map by combining sensor data with scan matching, and pose estimation is based on IMU data. When a certain number of point clouds generate a Submap, each time new point cloud data is inserted into the submap, the probability of the grid map will be updated.

[0135] The Cartographer slam algorithm is mainly divided into two parts: local SLAM and global SLAM. Local SLAM relies on the fused data of 2D LiDAR and RGB-D cameras to generate multiple submaps; while global SLAM integrates all submaps into a complete global map by combining sensor data with scan matching. Pose estimation is based on the precise odometer constructed in this paper.

[0136] The data sources of this algorithm include 2D LiDAR, RGB-D camera, odometer, and IMU. In local SLAM, when a new frame of fused point cloud data is obtained, it will be matched with the nearest submap to determine the optimal pose and construct the submap. Before inserting the point cloud data into the submap, coordinate transformation is required first. The specific transformation formula is as follows:

[0137]

[0138] where ε x represents the abscissa of the fused point cloud, ε y represents the ordinate of the fused point cloud, ε θ represents the observation angle of the fused point cloud, T ε represents the pose of the fused point cloud in the submap coordinate system, and p represents the probability of the grid having an obstacle.

[0139] When a certain number of point clouds generate a Submap, every time new point cloud data is inserted into the submap, the probability of the grid map will be updated. During this process, the pose (ε x , ε y , ε θ ) of the laser point cloud relative to the current submap is optimized by a Ceres-based scan matcher to find the optimal pose that maximizes the grid probability. The optimal pose is determined by calculating the matching error between the point cloud and the submap, and the Ceres scan matcher iteratively optimizes this pose to minimize the matching error. The matching error is calculated through the following residual function:

[0140]

[0141] where F smooth represents the smoothing function, and h n is the number of point clouds for fusion.

[0142] In global SLAM, Cartographer integrates all submaps into a complete map through loop detection and loop optimization, and eliminates the cumulative error between factor maps. Loop detection identifies loops by judging the similarity of the laser point poses between the current scan and the generated submaps. To improve the efficiency of loop detection and reduce the computational load, Cartographer adopts a branch and bound method to optimize the search strategy. In loop optimization, a sparse pose method is used to optimize the point cloud pose. The whole process is similar to the error calculation when laser point clouds are inserted into the submap, and the residual function is still used for error matching.

[0143] Figure 4 is a relationship diagram of a multi-sensor fusion control method for a blueberry greenhouse inspection vehicle according to an embodiment of the present invention.

[0144] As Figure 4 shown, the global path planned for each sub-region in the map is used as the input of the global planner of the navigation core of the robot operating system. Combining the blueberry greenhouse map (subscribing node / map) established based on the depth camera 72 and lidar 71 information of the inspection vehicle, the AMCL positioning information and the IMU sensor 73 message need to be integrated into the navigation core of the robot operating system after coordinate system conversion to form a global cost map. The local path planning DWA algorithm is used for real-time obstacle avoidance. Through the generated path information, the industrial control computer 21 calculates the real-time control instruction "cmd_vel" to guide it to avoid obstacles and move in the local environment and gradually approach the path specified by the global path planner to complete the inspection task of the inspection vehicle.

[0145] Figure 5 is a flowchart of a multi-sensor fusion path planning algorithm for a blueberry greenhouse inspection vehicle according to an embodiment of the present invention.

[0146] As shown Figure 5 in the figure, the A* algorithm is a heuristic search algorithm, which enables global path planning to determine the optimal path in a static network environment based on an evaluation function, as shown in the equation: f(n) = g(n) + h(n), where n represents the current node, f(n) is the integrated cost function, g(n) is the actual cost from the current node to the starting point, and h(n) is the heuristic function of the estimated cost from the current node to the target point. The Manhattan distance in the grid map is used to calculate the cost value, which represents the sum of the horizontal distance and the vertical distance from the current position node to the target point, as shown in the equation: |x n - x0| + |y n - y0|, where x n and x0 represent the horizontal coordinates of the current position node and the target point, while y n and y0 represent the vertical coordinates of the current position node and the target point, respectively.

[0147] The embodiments described above are only descriptions of the preferred embodiments of the present invention and do not limit the scope of the present invention. Without departing from the design spirit of the present invention, various modifications and improvements made by those of ordinary skill in the art to the technical solutions of the present invention shall fall within the protection scope determined by the claims of the present invention.

Claims

1. A control system for a blueberry greenhouse inspection vehicle with multi-sensor fusion, characterized in that, The system includes: a power supply module (9), a core control module (2), a chassis power module (4), an environment perception module (7), and a remote control module (1); The power supply module (9) is used to provide the required power for the system; The environment perception module (7) is used to obtain the environmental information of the blueberry greenhouse operation area where the inspection vehicle is located, construct an environmental map, detect surrounding obstacles, and achieve real-time obstacle avoidance; The core control module (2) is used to generate a path and control instructions according to the sensor data of the environment perception module (7), and transmit the control information to the chassis power module; The chassis power module (4) is used to drive the inspection vehicle to move according to the received control information; The remote control module (1) is used for the operator to manually control the inspection vehicle through a remote controller.

2. The system according to claim 1, wherein The core control module (2) consists of an industrial control computer (21), a single-chip microcomputer (22), and a remote control system (23); The industrial control computer (21) is connected to the single-chip microcomputer (22) through a CAN bus to realize information exchange; in the same local area network environment, the operator remotely logs in to the industrial control computer (21) through the SSH protocol to start the robot operating system, thereby realizing distributed remote control of the system.

3. The system according to claim 2, wherein The chassis power module (4) includes a motor drive module (41), a motor (42), and a walking track (43); The motor drive module (41) is connected to the pins of the motor (42) through the interface of the motor (42), and the drive signal pin of the motor drive module (41) is connected to the GPIO pin of the single-chip microcomputer (22). The motor (42) is connected to the walking track (43) through a mechanical structure to drive the track to realize the movement of the inspection vehicle.

4. The system according to claim 3, characterized in that, The environment perception module (7) includes a lidar (71), a depth camera (72), and an IMU sensor (73); The lidar (71) and the depth camera (72) are installed at the exact middle position in front of the inspection vehicle, and the IMU sensor (73) is installed at the center of the inspection vehicle and aligned with the symmetry axis of the inspection vehicle; The lidar (71), the depth camera (72), and the IMU sensor (73) jointly transmit data to the industrial control computer (21), and the industrial control computer (21) can perceive the greenhouse environment and collect environmental point cloud information; the IMU sensor (73) is responsible for detecting the pose of the inspection vehicle and making adjustments; the industrial control computer (21) processes the information using multi-sensor fusion algorithms, path planning, and obstacle avoidance decision algorithms, generates a greenhouse environment map, adjusts the pose of the inspection vehicle, and generates control information; finally, the industrial control computer (21) transmits the control information to the motor drive module (41) through the single-chip microcomputer (22) to realize that the inspection vehicle executes the inspection task according to the predetermined path and performs real-time obstacle avoidance.

5. The system according to claim 4, wherein, The remote control module (1) consists of a model airplane remote controller (12) and a signal receiver (11); The signal receiver (11) is connected to the single-chip microcomputer (22) and is responsible for information transmission. The RC airplane remote controller (12) communicates with the signal receiver (11) through wireless signals, and the operator manually controls the inspection vehicle through the remote controller.

6. The system according to claim 5, wherein The power supply module (9) includes an inspection vehicle battery (91), a boost module (92), and a voltage stabilization module (93); The inspection vehicle battery (91) supplies power to the motor drive module (41) to drive the motor (42) to operate; the boost module (92) raises the voltage of the inspection vehicle battery (91) and supplies power to the industrial control computer (21) through a power cord; after the voltage stabilization module (93) stabilizes the battery voltage, it provides a stable power supply to the single-chip microcomputer (22).

7. A control method for a blueberry greenhouse inspection vehicle with multi-sensor fusion, characterized in that, It is implemented by using the multi-sensor fusion blueberry greenhouse inspection vehicle control system according to any one of claims 1-6. The method includes the following steps: S1. Initialize the hardware of the inspection vehicle control system; S2. Operate the inspection vehicle to walk between the greenhouse rows through the remote control module (1), collect point cloud data, and complete the data fusion of the depth camera (72) and the lidar (71); S3. Generate an environmental map based on the fused data by using the cartographer slam algorithm; S4. Generate a path for the inspection operation based on the environmental map; S5. Drive the inspection vehicle to perform greenhouse inspection operations and autonomous real-time obstacle avoidance according to the environmental perception module (7) and the chassis walking module, and complete the operation.

8. The method according to claim 7, wherein In the above S2, the method of operating the inspection vehicle to walk between the greenhouse rows through the remote control module (1), collecting point cloud data, and completing the data fusion of the depth camera (72) and the lidar (71) includes: S201. Complete the joint calibration of the lidar (71) and the depth camera (72) to obtain the spatial transformation relationship between the lidar (71) and the depth camera (72), and output a parameter file of the joint calibration; S202. Use the lidar (71) to obtain surrounding environment data and transmit it to the industrial control computer (21), and calculate the distance between the inspection vehicle and the obstacle; S203. Use the depth camera (72) to obtain surrounding environment data and transmit it to the industrial control computer (21), identify the type of obstacle and calculate the depth information of the obstacle; S204. Use the depth camera (72) to convert the depth information into lidar-like data and fuse it with the lidar (71) data.

9. The method according to claim 7, wherein In the above S4, the method of generating a path for the inspection operation based on the environmental map includes: S401. The user operates the remote control terminal to remotely run the path planning launch file and enter the path planning operation mode; S402. Read the environmental model map, access the coupled data of the lidar (71) and the depth camera (72) for positioning, and access the data of the IMU sensor (73) to adjust the pose of the inspection vehicle; S403. Use the A* algorithm for path planning; S404. Generate a rasterized environmental model map and save the map; S405. End the process of the mapping launch file and exit the environment mapping mode.

10. The method according to claim 7, wherein In step S5, the method for driving the inspection vehicle to perform greenhouse inspection operations and autonomous real-time obstacle avoidance according to the environment perception module (7) and the chassis walking module includes: S501. The user remotely starts the autonomous obstacle avoidance launch file through the remote control terminal to enter the autonomous obstacle avoidance mode; S502. Read the gridded environmental model map, receive the data coupled by the lidar (71) and the depth camera (72), and perform multi-sensor fusion positioning to complete the positioning of the inspection vehicle of the lidar (71), the depth camera (72) and the IMU sensor; S503. The depth camera (72) uses the YOLOv5 network to identify the types of obstacles, calculates the depth information of the target obstacles, and combines the obstacle distance information provided by the lidar (71) to improve the positioning accuracy; S504. When the inspection vehicle encounters an obstacle, enable the local path planning DWA algorithm to avoid the obstacle and resume to the original operation path; S505. When the inspection vehicle completes the inspection operation in the blueberry greenhouse, stop at the operation end point and end the inspection task.

Images