Orchard inspection robot navigation method based on multi-sensor fusion

Through multi-sensor fusion technology and intelligent path planning, the problems of low efficiency and incomplete perception of traditional orchard management are solved, and high-precision and efficient automation of orchard inspections are achieved.

CN118936480APending Publication Date: 2024-11-12ZHEJIANG UNIV OF TECH
View PDF 0 Cites 17 Cited by

Patent Information

Application Number
CN202411315567.8
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-09-20
Publication Date
2024-11-12

AI Technical Summary

Technical Problem

Traditional orchard management relies on manual inspection, which has low efficiency and insufficient accuracy. The existing agricultural robots have incomplete environmental perception during orchard inspections and insufficient navigation and obstacle avoidance capabilities, resulting in unsatisfactory inspection results.

Method used

Using multi-sensor fusion technology, integrating two-dimensional radar, GPS, depth camera, wheeled odometer and inertial sensor IMU, a high-precision environmental model is constructed through Kalman filtering algorithm and SLAM technology, and the optimal inspection path is generated, combined with improved A * and dynamic window algorithms perform global and local path planning to ensure that the robot can avoid obstacles in real time.

Benefits of technology

It realizes comprehensive perception and intelligent decision-making of the orchard environment, improves the automation and intelligence level of orchard inspections, improves the accuracy and efficiency of inspections, and ensures that the robot can complete the inspection tasks stably and safely.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118936480A_ABST
    Figure CN118936480A_ABST
Patent Text Reader

Abstract

An orchard inspection robot navigation method based on multi-sensor fusion comprises the following steps: step 1, pre-storing sensor data required by an orchard inspection robot, including data of a two-dimensional laser radar, a GPS, a depth camera, a wheel type odometer and an inertial sensor IMU; step 2, processing pre-stored sensor data required by the orchard inspection robot; 3, fusing the data of the two-dimensional laser radar, the GPS, the depth camera, the wheel type odometer and the inertial sensor IMU through a Kalman filtering algorithm, obtaining environment information with higher precision and robot state estimation after filtering fusion, constructing a high-precision three-dimensional environment model based on the fused data, and identifying and positioning an obstacle; and step 4, generating an optimal inspection path based on the real-time environment sensing data and a predetermined inspection task. According to the invention, the automation and intelligence level of orchard inspection is improved.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The invention belongs to the field of agricultural automation and robotics technology, and in particular, relates to an orchard inspection robot navigation method based on multi-sensor fusion. Background Art

[0002] With the development of agricultural modernization, orchard management has higher and higher requirements for efficiency and accuracy. Traditional orchard management mainly relies on manual operation, which is not only time-consuming and labor-intensive, but also greatly affected by human factors, and has problems such as low efficiency and imprecise management. In addition, due to the large area of ​​fruit tree planting and complex environment, manual inspections are difficult to obtain comprehensive and timely orchard information, thus affecting the overall management effect of the orchard.

[0003] In the traditional orchard management model, fruit farmers usually need to personally patrol the orchard to check the growth of fruit trees, the maturity of fruits, and whether there are diseases and pests. This method is not only time-consuming and labor-intensive, but also prone to omissions and misjudgments due to the subjectivity of manual judgment. In addition, as the area of ​​orchards expands, the efficiency and coverage of manual inspections are greatly reduced, which cannot meet the needs of efficient management of modern orchards.

[0004] In recent years, the application of robotics in the agricultural field has gradually increased, and agricultural robots have begun to be used for tasks such as planting, picking, and spraying pesticides. However, existing agricultural robots still have some shortcomings in orchard inspections: many existing inspection robots rely only on a single type of sensor (such as visual sensors or lidar), which makes it difficult to cope with the complex and changing orchard environment. The limitations of this single sensor will lead to incomplete environmental perception, affecting the accuracy and efficiency of inspections; there are many obstacles in the orchard environment, such as trees, fruits, and undulating terrain, and robots need to have strong navigation and obstacle avoidance capabilities. However, the performance of existing robots in navigation and obstacle avoidance is not ideal, and they are prone to collisions or lost paths; a large amount of sensor data needs to be processed during the inspection process to generate environmental models in real time and make decisions. Some existing robots have limited capabilities in data processing and fusion, resulting in unsatisfactory inspection results. Summary of the invention

[0005] In order to overcome the shortcomings of the existing technology such as low management efficiency, insufficient accuracy and high labor cost, the present invention provides an orchard inspection robot navigation method based on multi-sensor fusion to improve the automation and intelligence level of orchard inspection. By integrating multiple sensors such as two-dimensional radar, GPS, depth camera, odometer and IMU, and utilizing multi-sensor fusion technology to integrate data from different sensors, more comprehensive and accurate environmental perception information can be provided.

[0006] The technical solution adopted by the present invention to solve its technical problem is:

[0007] A navigation method for an orchard inspection robot based on multi-sensor fusion comprises the following steps:

[0008] Step 1: Pre-store the sensor data required by the orchard inspection robot, including data from two-dimensional laser radar, GPS, depth camera, wheel odometer, and inertial sensor IMU, to ensure the accuracy and consistency of data collection. The pre-stored data is used to calibrate and optimize the robot's environmental perception and positioning capabilities, calibrate multiple sensors, and synchronize the time and space of multiple sensors;

[0009] Step 2: Process the pre-stored sensor data required by the orchard inspection robot, use the wheel odometer and inertial sensor IMU to perform distortion correction on each point of the two-dimensional lidar data, correct the data error caused by movement, and ensure the accuracy and consistency of the radar data; the three-dimensional image data collected by the depth camera is converted into two-dimensional lidar data through voxelization technology to supplement and enhance environmental perception capabilities; the GPS module provides global positioning information to assist the robot in large-scale inspections and path planning;

[0010] Step 3: The data from the two-dimensional lidar, GPS, depth camera, wheel odometer and inertial sensor IMU are fused through the Kalman filter algorithm. After filtering and fusion, more accurate environmental information and robot state estimation are obtained. Based on the fused data, a high-precision three-dimensional environmental model is constructed to identify and locate obstacles.

[0011] Step 4: Generate the optimal inspection path based on real-time environmental perception data and scheduled inspection tasks. Use SLAM, a synchronous localization and mapping technology, to build an environmental map in real time and locate the robot itself. The SLAM-optimized map is used for path planning. Combined with the improved A * The global and local path planning is performed using the dynamic window algorithm to ensure that the robot can avoid obstacles in real time and perform stable inspections.

[0012] Furthermore, in step 1, the two-dimensional laser radar is a G4 laser radar, which transmits a laser beam and receives a reflected signal, calculates the time difference from the laser emission to the laser reception, and thus determines the distance to the target object. It has high measurement accuracy and stability, and uses triangulation to measure the distance, and the measurement distance is 0.28 to 16 meters.

[0013] In step 1, the GPS is Beidou RTK GPS, which has centimeter-level accuracy and can well locate the robot. Beidou RTK GPS improves positioning accuracy through differential correction between the reference station and the mobile station, and is suitable for outdoor orchard environments that require high-precision positioning.

[0014] In step 1, the depth camera is a high-precision, low-power 3D camera based on monocular structured light technology. This high-precision, low-power device generates a three-dimensional depth image by projecting a structured light pattern and capturing its deformation, and is suitable for fine measurement and recognition in complex orchard environments.

[0015] In step 1, the wheel odometer is a method that uses data obtained from a mobile sensor to estimate the change in the position of an object over time. It is used to estimate the distance the robot moves relative to the initial position. By recording the rotation amount of the wheel, the displacement of the robot on the plane is calculated to assist the robot in positioning and navigation.

[0016] In step 1, the inertial measurement unit (IMU) is a device for measuring the three-axis attitude angle and acceleration of an object. It monitors the motion state of the robot in real time through an accelerometer and a gyroscope, provides attitude and motion change information, and supplements and corrects the data of other sensors.

[0017] Furthermore, in step 2, the two-dimensional lidar data is distorted and corrected using a wheel odometer and an inertial measurement unit IMU. The inertial measurement unit IMU provides real-time acceleration and angular velocity information for calculating the robot's posture changes, and can estimate the posture changes of the lidar during scanning; the wheel odometer provides the robot's displacement information, including the forward distance and rotation angle, which is used to estimate the displacement changes of the lidar during scanning; combined with the posture and displacement change information provided by the IMU and wheel odometer, each laser point is distorted and corrected, and the corrected lidar data more accurately reflects the actual distance and angle relationship in the environment.

[0018] In step 2, the voxelization technology refers to voxelizing the three-dimensional point cloud data of the depth map. The voxelization technology divides the three-dimensional space into a number of small cubes, namely voxels, each voxel representing a fixed volume unit. In the voxelization process, each point in the three-dimensional point cloud data is assigned to a corresponding voxel. The data structure after voxelization is more regular, which is conducive to subsequent processing and analysis. The voxelization process can control the resolution and balance the amount of calculation by setting an appropriate voxel size.

[0019] In step 2, converting the depth map into two-dimensional lidar data means projecting the voxelized three-dimensional data onto a two-dimensional plane to simulate the two-dimensional lidar data; selecting a suitable projection plane, and recording the projection position and depth information of each voxel on the plane; the generated two-dimensional data contains the plane coordinates and depth value of each projection point, which is similar to the distance measurement data of the two-dimensional lidar. The above two data forms can provide the robot with distance information of the surrounding environment. The two-dimensional lidar directly measures the distance, and the depth map is obtained by projection calculation. The "similar" here is just that the generated data is similar in structure and function.

[0020] In step 2, each pixel in the depth map is converted into point cloud data in three-dimensional space according to the internal and external parameters of the camera obtained in step 1.

[0021] X=(u-cx)×d / f x

[0022] Y=(v-cy)×d / f y

[0023] Z=d

[0024] (X, Y, Z): 3D point cloud coordinates

[0025] (u, v): pixel coordinates,

[0026] f x , f y : Camera focal length

[0027] cx, cy: optical center coordinates

[0028] d: depth value.

[0029] In step 2, a projection plane is selected, and the three-dimensional point cloud data is projected onto the plane, so as to obtain the distance information of the two-dimensional plane; this process actually removes the value of a certain coordinate axis of the three-dimensional point and only retains the values ​​of the other two coordinate axes. If a horizontal plane is selected as the projection plane, the height information can be ignored and projected onto the XY plane;

[0030] x′=X

[0031] y′=Y

[0032] x′, y′: projection point.

[0033] In step 2, the projected two-dimensional point cloud data is processed into a laser radar format. By calculating the distance and angle between each projection point and the robot, two-dimensional data similar to the laser radar can be generated. The two-dimensional data of the laser radar is usually expressed in polar coordinates. Each point is described by distance and angle parameters, representing the spatial position of the point relative to the laser radar sensor. In the projected two-dimensional point cloud data, the distance and angle of each projection point relative to the robot can also be obtained by calculation. After calculating the distance and angle of each projection point, the generated data can be organized into a two-dimensional data format similar to the laser radar.

[0034] The projection point is (x′, y′), then the distance r and angle θ are calculated by the following formula, and these distance and angle information are organized into a two-dimensional data format;

[0035]

[0036] θ=tan -1 (y′ / x′).

[0037] In step 2, the real-time global position information of the robot is obtained through the GPS module, and these data are converted into global coordinates that can be used by the robot to help determine its current location; based on the GPS positioning information, the entire orchard inspection range is divided into several inspection areas, and robots can be assigned to perform inspection tasks in turn according to these areas to ensure maximum coverage.

[0038] Furthermore, in step 3, the collected two-dimensional laser radar and depth camera data are corrected and filtered, and then fused with the GPS data through Kalman filtering, the Kalman filter is initialized, the initial position, speed, acceleration, attitude and other state variables are set, the initial uncertainty is set, the noise model is set according to the sensor performance, and the motion model is used to predict the state;

[0039]

[0040] X K|K-1 : Predicted status

[0041] F: state transfer matrix

[0042] B: Control input matrix

[0043] u k-1 : Control input.

[0044] In step 3, the updating steps of the Kalman filter are as follows:

[0045] K k =P k|k-1 H T (HP k|k-1 H T +R) -1

[0046]

[0047] P k|k =(IK k H)P k|k-1

[0048] K k : Kalman gain

[0049] Z k : Measured value

[0050] H: measurement matrix

[0051] R: measurement noise covariance matrix.

[0052] In step 3, a complete three-dimensional point cloud is generated using the depth camera and LiDAR data, the point cloud is filtered and registered, noise and redundant data are removed, an environmental model is constructed based on the three-dimensional point cloud, a three-dimensional map is generated, and the fused data generates a high-precision environmental map.

[0053] In step 4, relying on the environmental map constructed in step 3, the improved A* algorithm is used for global path planning. The improved A* algorithm is optimized on the cost function, taking into account factors such as the priority of the robot's inspection task, the length of the path, the energy consumption of the robot's travel, and dangerous areas in the environment, and dynamically adjusts and optimizes the path; in this way, a global optimal path can be generated that avoids unnecessary complex sections and gives priority to inspection target points.

[0054] In step 4, during the inspection process, obstacles in the environment may change dynamically. The dynamic window algorithm DWA is used to calculate the local feasible path in real time based on the robot's current motion state (such as speed, direction, etc.) and perceived environmental information. When the robot is about to approach an obstacle, the motion trajectory is automatically adjusted to enable the robot to avoid dynamic obstacles and ensure the real-time and safety of path planning.

[0055] In step 4, global and local path planning are combined, and the global path planning provides an optimal route from the starting point to the inspection task target point; when the robot senses changes in the environment (such as the appearance of new obstacles) during execution, the local path planning will respond in real time and update the local path to ensure that the robot can bypass the obstacles and continue to move forward; at the same time, the adjustment of the local path will not affect the completion of the global path. The goal of local path planning is to achieve optimal obstacle avoidance in the current task environment; the cooperation of the two ensures that when the robot completes the inspection task, it can not only follow the globally planned route, but also respond to real-time emergencies.

[0056] The beneficial effects of the present invention are mainly manifested in: the present invention utilizes multi-sensor fusion technology to integrate data from different sensors to provide more comprehensive and accurate environmental perception information; by integrating multiple sensors such as two-dimensional radar, GPS, depth camera, odometer and IMU, comprehensive perception and intelligent decision-making of the orchard environment are achieved, thereby improving the automation and intelligence level of orchard inspection. BRIEF DESCRIPTION OF THE DRAWINGS

[0057] Figure 1 This is a schematic diagram of the hardware architecture of the orchard inspection robot navigation method based on multi-sensor fusion.

[0058] Figure 2 It is a flowchart of the navigation method of the orchard inspection robot based on multi-sensor fusion. DETAILED DESCRIPTION

[0059] The present invention will be further described below in conjunction with the accompanying drawings.

[0060] Reference Figure 1 and Figure 2 , a navigation method for an orchard inspection robot based on multi-sensor fusion. In the hardware architecture for implementing this method, the main controller uses NVIDIA Jetson NanoA02, which uses a quad-core 64-bit ARM CPU and a 128-core integrated NVIDA GPU with multiple communication interfaces, and can process information in real time. The underlying motion control uses an STM32 controller, which is connected to multiple DC encoder motors to achieve the robot's underlying motion control and sensor data acquisition. The main controller communicates with the underlying motion controller through a serial port protocol. The main controller and the radar are connected via USB to complete data transmission, and the main controller and the depth camera are connected via USB. The wheel odometer is set in the motion carrier module, and the IMU is built into the underlying motion controller.

[0061] The main controller reads the data of the three-axis accelerometer and three-axis gyroscope in the robot through the serial port, and determines the working status of the robot based on the read data. The main controller and the PC transmit data via WIFI. The PC displays the movement status of the robot and receives data from various sensors to debug the robot.

[0062] Reference Figure 2 , a navigation method for an orchard inspection robot based on multi-sensor fusion, the method comprising the following steps:

[0063] Step 1: Pre-store the sensor data required by the orchard inspection robot, including data from two-dimensional laser radar, GPS, depth camera, wheel odometer, and inertial sensor IMU, to ensure the accuracy and consistency of data collection. The pre-stored data is used to calibrate and optimize the robot's environmental perception and positioning capabilities, calibrate multiple sensors, and synchronize the time and space of multiple sensors;

[0064] First, multiple sensors are spatially calibrated, including the following steps:

[0065] Step 1.1.1, depth camera intrinsic parameter calibration: Place the calibration plate in the field of view of the depth camera, use the calibration tool camera_calibration to take multiple images containing the calibration plate, determine the camera's intrinsic parameters including focal length, optical center position, distortion coefficient, etc., and correct the depth camera's intrinsic parameter error to ensure the accuracy of the captured image;

[0066] Step 1.1.2, depth camera external parameter calibration: Use the calibrated calibration board to determine the posture and position of the depth camera in space according to the image data obtained by the depth camera, and calculate the rotation matrix and displacement vector of the depth camera;

[0067] Step 1.1.3, LiDAR calibration: Install the LiDAR and the depth camera on the same rigid platform to ensure that their positions are relatively fixed during the calibration process, and that both the LiDAR and the depth camera can see the calibration plate at the same time. Use the wheel odometer and IMU data to correct the measurement error of the LiDAR and correct the data deviation caused by the robot movement;

[0068] Step 1.1.4, joint calibration: Place the calibration plate in the common field of view of the LiDAR and the depth camera, and collect data from the LiDAR and the depth camera at the same time. Extract the feature points of the calibration plate from the 3D point cloud data of the depth camera, and extract the feature points at the same position from the 2D scanning data of the LiDAR. Project the 2D data of the LiDAR into the 3D space, and represent it using the same world coordinate system as the depth camera. Through coordinate transformation, calculate the rotation matrix and translation vector between the LiDAR and the depth camera. Use the least squares algorithm to match the data of the LiDAR and the depth camera to minimize their measurement errors on the feature points of the calibration plate. Solve the linear equation to get the calibration parameters.

[0069] Synchronize the timestamps of multiple sensors and use ApproximateTime in ROS to synchronize the timestamps of lidar, depth camera, IMU and wheel odometer, including the following steps:

[0070] Step 1.2.1, store the sensor data into the stack structure according to the data order transmitted in the data approximate synchronization procedure;

[0071] Step 1.2.2, extract the data stamps in the latest stack for comparison. If the difference is large, use the ROS current time to assign the stack and fuse the nearby sensors at this time point together;

[0072] Step 1.2.3, repeat the above steps.

[0073] Step 2: Process the pre-stored sensor data required by the orchard inspection robot, use the wheel odometer and inertial sensor IMU to perform distortion correction on each point of the two-dimensional lidar data, correct the data error caused by movement, and ensure the accuracy and consistency of the radar data; the three-dimensional image data collected by the depth camera is converted into two-dimensional lidar data through voxelization technology to supplement and enhance environmental perception capabilities; the GPS module provides global positioning information to assist the robot in large-scale inspections and path planning;

[0074] The following steps are used to correct the laser radar distortion using IMU and wheel odometer:

[0075] Step 2.1.1, data collection: LiDAR provides point cloud data, IMU provides six-degree-of-freedom motion data, including acceleration and angular velocity, which are used to estimate posture changes, and wheel odometer provides wheel-based displacement and rotation information to help estimate the relative displacement of the robot;

[0076] Step 2.1.2, according to the timestamp synchronization in the previous step, synchronize the IMU, wheel odometer and lidar;

[0077] Step 2.1.3, using the angular velocity and acceleration of the IMU, combined with the displacement data of the wheel odometer, a more accurate motion trajectory can be obtained, including the estimation of the robot's translation and rotational motion;

[0078] Step 2.1.4: It takes a certain amount of time for the laser radar to scan a complete frame, which will cause different sampling times for each point cloud point, thus causing distortion. Using the motion estimation of the IMU and wheel odometer, the timestamp of each laser point is interpolated or extrapolated to obtain the coordinate change of the point cloud point, thereby correcting the distorted point cloud data to a unified coordinate system.

[0079] Converting depth camera images (3D image data) to 2D radar data involves the following steps:

[0080] Step 2.2.1, use the depth image to convert the two-dimensional pixels into three-dimensional space coordinates, and use the following formula to map the pixel points to three-dimensional points:

[0081]

[0082]

[0083] Z = Depth(u,v)

[0084] (u, v) is the pixel position in the image

[0085] (X, Y, Z) is a three-dimensional point in the world coordinate system

[0086] Z is the depth value, indicating the distance from the point to the camera

[0087] Step 2.2.2, divide the three-dimensional space into a uniform voxel grid by voxelization. Each voxel represents a three-dimensional cube of fixed size, containing a certain amount of point cloud information. i , Y i , Z i ) calculates the voxel coordinates where it is located;

[0088]

[0089] Calculate the average depth for each voxel unit

[0090] N points is the number of points within a voxel unit, Z i is the depth value of each point

[0091] Step 2.2.3, project the 3D voxels onto the ground plane. Ignore the height information and only keep the horizontal (X, Y) position and the corresponding depth information. For each voxel unit (i, j, k), calculate its projection point (X proj , Y proj ),

[0092] X proj =i·d x , Y proj =j·d y

[0093] The two-dimensional projection points of all voxels are placed in a two-dimensional grid, and the size of the grid is determined by the range of the projection points. Each grid unit stores the depth information of its corresponding voxel, and a two-dimensional depth map is obtained, which is similar to the two-dimensional scanning result of LiDAR;

[0094] Step 2.2.4, the projection data on the two-dimensional plane is further converted into polar coordinate format, and each projection point (X proj , Y proj ) is converted to

[0095] r is the distance, θ is the angle

[0096] For the projection point of each voxel unit, the distance and angle to the origin are calculated, and these data are arranged according to the angle to generate lidar-like scanning data.

[0097] Data correction using the global positioning information provided by the GPS module is usually used to improve the positioning accuracy of robot navigation. The sensor data is corrected by a filter, and the GPS data is used as a benchmark to gradually correct the accumulated drift error of the sensor. The corrected position data is more accurate than using only a single sensor and can be used as a basis for further navigation decisions.

[0098] Step 3: The data from the two-dimensional lidar, GPS, depth camera, wheel odometer and inertial sensor IMU are fused through the Kalman filter algorithm. After filtering and fusion, more accurate environmental information and robot state estimation are obtained. Based on the fused data, a high-precision three-dimensional environmental model is constructed to identify and locate obstacles.

[0099] The Kalman filter realizes multi-sensor data fusion through two steps of prediction and update, including the following steps:

[0100] Step 3.1, define the robot's current state vector x t =[x,y,θ,v x , v y , w]

[0101] x, y are the position coordinates of the robot, θ is the current heading angle of the robot, v x , v y is the speed of the robot in the x and y directions, and w is the angle of rotation of the robot;

[0102] Step 3.2, the acceleration, angular velocity, speed and other data provided by the IMU and wheel speed meter are used as the control input u t , using the global location information provided by GPS to update the state z t , use the motion model to predict the robot's state x t+1 =f(x t ,u t )+w t ;

[0103] f(x t ,u t ) is the state transfer function, w t is the process noise

[0104] z t =h(x t )+v t

[0105] z t is the GPS location information, h(x t ) is the predicted observation value, v t is the observation noise

[0106] Step 3.3, use the distance data provided by the lidar to scan the local environment, generate point cloud data, and adjust the robot position estimate by matching these point clouds;

[0107] Step 3.4, calculate the Kalman gain K t , to update the robot state in combination with the observed information, the update equation is x t =x t|t-1 +K t (z t -h(x t|t-1 )).

[0108] Step 4: Generate the optimal inspection path based on real-time environmental perception data and scheduled inspection tasks. Use simultaneous localization and mapping technology (SLAM) to build an environmental map in real time and locate the robot itself. The SLAM-optimized map is used for path planning. Combined with the improved A* and dynamic window algorithms, global and local path planning is performed to ensure that the robot can avoid obstacles in real time and perform stable inspections.

[0109] In step 4, relying on the environmental map constructed in step 3, the improved A* algorithm is used for global path planning. The improved A* algorithm is optimized on the cost function, taking into account factors such as the priority of the robot inspection task, the length of the path, the energy consumption of the robot's travel, and the dangerous areas in the environment, and dynamically adjusting and optimizing the path; in this way, a global optimal path can be generated that avoids unnecessary complex sections and gives priority to inspection target points;

[0110] During the inspection process, obstacles in the environment may change dynamically. The dynamic window algorithm DWA is used to calculate the local feasible path in real time based on the current motion state of the robot and the perceived environmental information. When the robot is about to approach an obstacle, the motion trajectory is automatically adjusted to enable the robot to avoid dynamic obstacles and ensure the real-time and safety of path planning. 10. A navigation method for an orchard inspection robot based on multi-sensor fusion as described in claim 9, characterized in that in step 4, the combination of global and local path planning, the global path planning provides an optimal route from the starting point to the inspection task target point; when the robot senses changes in the environment during execution (such as the appearance of new obstacles), the local path planning will respond in real time and update the local path to ensure that the robot can bypass the obstacles and continue to move forward; at the same time, the adjustment of the local path will not affect the completion of the global path, and the goal of the local path planning is to achieve optimal obstacle avoidance in the current task environment; the cooperation of the two ensures that the robot can follow the global planning route and respond to real-time emergencies when completing the inspection task.

[0111] The Cartographer algorithm is used to build SLAM maps. The map is built in real time using lidar data. The current lidar scan is matched with the existing map by iterating the nearest point, and the robot's position in the map is updated. The loop detection technology is used to identify the areas that the robot has passed through, so as to reduce the possible cumulative errors in the SLAM process.

[0112] In this embodiment, based on the terrain of the orchard and known obstacles, an improved A *The algorithm performs global planning and generates the optimal path from the starting point to the end point based on the map, ensuring that the robot's inspection path in the entire orchard is feasible and has the highest coverage. As the robot moves forward, the dynamic window algorithm DWA is executed in real time to perform local obstacle avoidance. DWA evaluates multiple possible motion trajectories and selects the optimal trajectory to avoid obstacles while ensuring speed.

[0113] Specifically, the heuristic weights are dynamically adjusted based on real-time obstacle detection information. When the robot detects an obstacle ahead, the heuristic weight increases the impact of the obstacle on the path, thereby avoiding areas with dense obstacles. In the orchard, the robot needs to travel on a smoother path, and Bezier curves or spline interpolation are used to smooth the path, making the robot travel more smoothly and reducing sharp turns in the path. When an obstacle appears on a pre-planned path, the path is adjusted through local replanning without having to recalculate the global path. If a dynamic obstacle appears in front of the robot, DWA will adjust the robot's direction and speed in real time on the current path to avoid collisions, ensuring that the robot can flexibly respond to complex environmental changes.

[0114] By implementing the above scheme, the present method can:

[0115] 1. Fuse data from sensors such as GPS, IMU, LiDAR, and depth cameras to improve the accuracy of robot positioning. Especially when the GPS signal is poor or fails, the fusion of SLAM and sensors can maintain the robot's accurate perception of its position.

[0116] 2. Through the real-time feedback control system, the robot adjusts its motion parameters in time according to the sensor's perception data to ensure stable driving during the inspection process and will not deviate from the inspection task due to short-term errors or interference from the sensor.

[0117] 3. Combining global path planning with local obstacle avoidance algorithms ensures that the robot can effectively cover all areas of the orchard, especially in areas with dense trees or narrow roads, to avoid omissions.

[0118] The contents described in the embodiments of this specification are merely enumerations of implementation forms of the inventive concept and are for illustrative purposes only. The protection scope of the present invention should not be considered to be limited to the specific forms described in this embodiment, and the protection scope of the present invention also extends to equivalent technical means that can be thought of by ordinary technicians in this field based on the inventive concept.

Claims

1. A navigation method for an orchard inspection robot based on multi-sensor fusion, characterized in that: The method comprises the following steps: Step 1: Pre-store the sensor data required by the orchard inspection robot, including data from two-dimensional laser radar, GPS, depth camera, wheel odometer, and inertial sensor IMU, to ensure the accuracy and consistency of data collection. The pre-stored data is used to calibrate and optimize the robot's environmental perception and positioning capabilities, calibrate multiple sensors, and synchronize the time and space of multiple sensors; Step 2: Process the pre-stored sensor data required by the orchard inspection robot, use the wheel odometer and inertial sensor IMU to perform distortion correction on each point of the two-dimensional lidar data, correct the data error caused by movement, and ensure the accuracy and consistency of the radar data; the three-dimensional image data collected by the depth camera is converted into two-dimensional lidar data through voxelization technology to supplement and enhance environmental perception capabilities; the GPS module provides global positioning information to assist the robot in large-scale inspections and path planning; Step 3: The data from the two-dimensional lidar, GPS, depth camera, wheel odometer and inertial sensor IMU are fused through the Kalman filter algorithm. After filtering and fusion, more accurate environmental information and robot state estimation are obtained. Based on the fused data, a high-precision three-dimensional environmental model is constructed to identify and locate obstacles. Step 4: Generate the optimal inspection path based on real-time environmental perception data and scheduled inspection tasks. Use SLAM, a synchronous localization and mapping technology, to build an environmental map in real time and locate the robot itself. The SLAM-optimized map is used for path planning. Combined with the improved A * The global and local path planning is performed using the dynamic window algorithm to ensure that the robot can avoid obstacles in real time and perform stable inspections.

2. The orchard inspection robot navigation method based on multi-sensor fusion as claimed in claim 1, characterized in that: In step 1, the two-dimensional laser radar is a G4 laser radar, which transmits a laser beam and receives a reflected signal, calculates the time difference from the laser emission to the laser reception, and thus determines the distance to the target object. It has high measurement accuracy and stability, and uses triangulation to measure the distance, and the measurement distance is 0.28 to 16 m. The GPS is Beidou RTK GPS, which has centimeter-level accuracy and can well locate the robot. Beidou RTK GPS improves positioning accuracy through differential correction between the reference station and the mobile station, and is suitable for outdoor orchard environments that require high-precision positioning; The depth camera is a high-precision, low-power 3D camera based on monocular structured light technology. This high-precision, low-power device generates a three-dimensional depth image by projecting a structured light pattern and capturing its deformation, and is suitable for fine measurement and recognition in complex orchard environments. The wheel odometer is a method that uses data obtained from mobile sensors to estimate the change in the position of an object over time. It is used to estimate the distance the robot moves relative to the initial position. By recording the rotation amount of the wheel, the displacement of the robot on the plane is calculated to assist the robot in positioning and navigation. The inertial measurement unit (IMU) is a device that measures the three-axis attitude angle and acceleration of an object. It monitors the robot's motion state in real time through an accelerometer and a gyroscope, provides attitude and motion change information, and supplements and corrects data from other sensors.

3. The orchard inspection robot navigation method based on multi-sensor fusion as described in claim 1 or 2, characterized in that: In step 2, the two-dimensional lidar data is distorted and corrected using a wheel odometer and an inertial measurement unit (IMU). The inertial measurement unit (IMU) provides real-time acceleration and angular velocity information for calculating the robot's posture changes, and can estimate the posture changes of the lidar during scanning. The wheel odometer provides displacement information of the robot, including the forward distance and rotation angle, which is used to estimate the displacement changes of the lidar during scanning. Combined with the posture and displacement change information provided by the IMU and wheel odometer, each laser point is distorted and corrected, and the corrected lidar data more accurately reflects the actual distance and angle relationship in the environment.

4. The orchard inspection robot navigation method based on multi-sensor fusion as described in claim 1 or 2, characterized in that: In step 2, the voxelization technology refers to voxelizing the three-dimensional point cloud data of the depth map. The voxelization technology divides the three-dimensional space into a number of small cubes, namely voxels, and each voxel represents a fixed volume unit. In the voxelization process, each point in the three-dimensional point cloud data is assigned to a corresponding voxel. The data structure after voxelization is more regular, which is conducive to subsequent processing and analysis. The voxelization process can control the resolution and balance the amount of calculation by setting an appropriate voxel size.

5. The orchard inspection robot navigation method based on multi-sensor fusion as described in claim 1 or 2, characterized in that: In the step 2, converting the depth map into two-dimensional laser radar data means projecting the voxelized three-dimensional data onto a two-dimensional plane to simulate the two-dimensional laser radar data; selecting a projection plane, and recording the projection position and depth information of each voxel on the plane; The generated two-dimensional data contains the plane coordinates and depth value of each projection point.

6. The orchard inspection robot navigation method based on multi-sensor fusion as claimed in claim 1 or 2, characterized in that: In step 2, each pixel in the depth map is converted into point cloud data in three-dimensional space according to the internal and external parameters of the camera obtained in step 1. X=(u-cx)×d / f x Y=(v-cy)×d / f y Z=d (X, Y, Z): 3D point cloud coordinates (u, v): pixel coordinates, f x , f y : Camera focal length cx, cy: optical center coordinates d: depth value.

7. The orchard inspection robot navigation method based on multi-sensor fusion as described in claim 1 or 2, characterized in that: In the step 2, a projection plane is selected, and the three-dimensional point cloud data is projected onto the plane, thereby obtaining distance information of the two-dimensional plane; This process actually removes the value of one coordinate axis of the three-dimensional point and only retains the values ​​of the other two coordinate axes. If you choose the horizontal plane as the projection plane, you can ignore the height information and project it to the XY plane. x′=X y′=Y x′, y′: projection point; The projected two-dimensional point cloud data is processed into the format of the laser radar. By calculating the distance and angle between each projection point and the robot, two-dimensional data can be generated. The projection point is (x′, y′), then the distance r and angle θ are calculated by the following formula, and these distance and angle information are organized into a two-dimensional data format; The real-time global position information of the robot is obtained through the GPS module, and these data are converted into global coordinates that the robot can use to help determine its current location; based on the GPS positioning information, the entire orchard inspection range is divided into several inspection areas, and robots can be assigned to perform inspection tasks in turn according to these areas to ensure maximum coverage.

8. The orchard inspection robot navigation method based on multi-sensor fusion as claimed in claim 1 or 2, characterized in that: In step 3, the collected two-dimensional laser radar and depth camera data are corrected and filtered, and then fused with the GPS data through Kalman filtering, the Kalman filter is initialized, the initial position, speed, acceleration, attitude and other state variables are set, the initial uncertainty is set, the noise model is set according to the sensor performance, and the motion model is used to predict the state; X K|K-1 : Predicted status F: state transfer matrix B: Control input matrix u k-1 : Control input; The update steps of Kalman filter are as follows: K k =P k|k-1 H T (HP k|k-1 H T +R) -1 P k|k =(I-K k H)P k|k-1 K k : Kalman gain Z k : Measured value H: measurement matrix R: measurement noise covariance matrix; Use depth camera and LiDAR data to generate a complete 3D point cloud, filter and align the point cloud, remove noise and redundant data, build an environmental model based on the 3D point cloud, generate a 3D map, and generate a high-precision environmental map with the fused data.

9. The orchard inspection robot navigation method based on multi-sensor fusion as claimed in claim 1 or 2, characterized in that: In step 4, based on the environment map constructed in step 3, the improved A * The algorithm performs global path planning. The improved A * The algorithm optimizes the cost function, taking into account factors such as the priority of the robot's inspection task, the length of the path, the energy consumption of the robot's movement, and the dangerous areas in the environment, and dynamically adjusts and optimizes the path; this can generate a global optimal path that avoids unnecessary complex sections and gives priority to inspection target points; During the inspection process, obstacles in the environment may change dynamically. The dynamic window algorithm DWA is used to calculate the local feasible path in real time based on the robot's current motion state and perceived environmental information. When the robot is about to approach an obstacle, the motion trajectory is automatically adjusted to enable the robot to avoid dynamic obstacles and ensure the real-time and safety of path planning.

10. The orchard inspection robot navigation method based on multi-sensor fusion as claimed in claim 9, characterized in that: In step 4, global and local path planning are combined, and the global path planning provides an optimal route from the starting point to the inspection task target point; when the robot senses changes in the environment (such as the appearance of new obstacles) during execution, the local path planning will respond in real time and update the local path to ensure that the robot can bypass the obstacles and continue to move forward; at the same time, the adjustment of the local path will not affect the completion of the global path. The goal of local path planning is to achieve optimal obstacle avoidance in the current task environment; the cooperation of the two ensures that when the robot completes the inspection task, it can not only follow the globally planned route, but also respond to real-time emergencies.

Citation Information

Cited By

  • Inspection robot inspection track self-correction method based on SLAM (Simultaneous Localization and Mapping) navigation

    CN119369434A

  • Three-dimensional terrain modeling analysis and adaptive navigation system and method

    CN119594971A

  • Inspection task real-time optimization scheduling method based on artificial intelligence

    CN119671176A

  • Lightning-vision fusion type traffic safety facility bus patrolling method and system oriented to low-signal scene

    CN119723891A

  • Foot type robot terrain sensing method and system based on multi-sensor fusion

    CN119737939A