UWB navigation system and method based on visual feature fusion

Abstract

The application discloses a UWB navigation system and method based on visual feature fusion, and the system comprises four modules of UWB positioning, visual perception, data fusion processing and navigation control. The UWB positioning module adaptively generates a UWB anchor point layout, and after calculating UWB coordinates in real time, UWB positioning data are obtained by calibration according to the coverage rate of fruit trees and soil humidity. The visual perception module collects orchard images, identifies fruit trees and obstacles, obtains visual coordinates and corrects visual positioning data. The data fusion processing module calculates the weight of UWB positioning data and visual positioning data, fuses UWB positioning data and visual positioning data according to the weight to obtain fusion coordinates, and corrects errors to obtain optimal coordinates. The navigation control module calculates target path points according to the optimal coordinates, obstacle recognition results and weeding operation paths, and controls the weeding robot to travel to the target path points. The application realizes high-precision navigation of the weeding robot in the complex environment of the orchard.

Description

Technical Field

[0001] This invention relates to the field of robot navigation technology, and specifically to a UWB navigation system and method based on visual feature fusion. Background Technology

[0002] In the development of modern agriculture, orchard weeding robots are being used more and more widely, and the accuracy and reliability of their navigation technology play a decisive role in operational efficiency and effectiveness. Currently, the navigation technologies commonly used in orchard weeding robots mainly include ultra-wideband (UWB) positioning technology and visual navigation technology.

[0003] UWB positioning technology, with its centimeter-level positioning accuracy, strong multipath resistance, and high temporal resolution, has found some applications in indoor and outdoor positioning. Deploying UWB anchor points in orchard scenarios, with robots equipped with UWB tags, can achieve relatively accurate location determination. However, the orchard environment is complex and variable; branches and trunks of fruit trees can obstruct UWB signals, causing multipath effects and increasing positioning errors. Furthermore, UWB positioning relies on pre-deployed anchor points; for orchards with irregular terrain and large areas, anchor point deployment is costly and it's difficult to guarantee full coverage, making effective positioning impossible in anchor point signal blind spots.

[0004] Visual navigation technology acquires environmental images through cameras and processes and analyzes these images using computer vision algorithms, enabling environmental perception, feature extraction, and localization. It requires no additional complex hardware deployment and can identify environmental information such as obstacles and rows of fruit trees, providing a basis for path planning. However, this technology has significant limitations. For example, it is heavily dependent on lighting conditions; in poor lighting environments such as nighttime, rainy days, or direct sunlight, image quality deteriorates, making feature extraction difficult and significantly reducing localization accuracy. Furthermore, pure visual localization algorithms (such as visual SLAM) accumulate errors over long periods of operation, affecting the accuracy and stability of navigation, making it difficult to meet the high-precision, long-term operation requirements of orchard weeding robots.

[0005] Currently, although there is some research combining multiple navigation technologies, existing multi-sensor fusion navigation technology in orchard scenarios still has the following shortcomings:

[0006] (a) The contradiction between signal stability and environmental adaptability: multipath effects and blockages lead to positioning faults

[0007] 1. Orchard-specific interference with UWB signals remains unresolved.

[0008] Existing solutions only use general EKF filtering to process UWB data. However, the dense tree trunks and branches in orchards cause multipath reflections and superposition of UWB signals. At the same time, changes in soil moisture exacerbate signal attenuation, causing the UWB positioning error to jump from the centimeter level to more than ±30cm. Most solutions do not optimize the filtering algorithm for orchard scenarios and only rely on increasing the number of anchor points to improve accuracy, which actually increases hardware costs by more than 40%.

[0009] 2. The visual module lacks robustness to orchard light / shading conditions.

[0010] While existing visual navigation systems employ improved deep learning-based target detection algorithms, they suffer from two major limitations: first, strong light saturation and shadow interference—the camera's dynamic range is insufficient under direct midday sunlight, leading to mismatches of over 30% of fruit tree feature points; second, weed and branch occlusion—when weed coverage exceeds 50%, the visual algorithm easily misidentifies weeds as obstacles or fruit tree trunks, resulting in navigation line fitting deviations of ±15cm. Furthermore, most solutions do not consider the dynamic growth of fruit trees, and fixed visual parameters can cause a 50% decrease in positioning accuracy after three months.

[0011] (ii) The contradiction between fusion logic and scenario adaptability: static weights cannot cope with sudden environmental changes.

[0012] 1. The weight allocation for fusion lacks a dynamic adjustment mechanism.

[0013] When the UWB signal strength is below -85dBm (common in densely fruit-growing areas) or the visual matching degree is below 60% (on cloudy or rainy days), the weight adjustment still relies on general rules, resulting in a lag in the fusion positioning response, which cannot meet the real-time requirements of the robot at a working speed of 0.5m / s.

[0014] 2. Coordinate system calibration ignores orchard topographic deviations.

[0015] The existing solution uses trajectory matching to unify the UWB and visual coordinate system. However, the orchard has micro-topographical undulations and soil subsidence. Fixed coordinate system transformation parameters will lead to cumulative errors. After 4 hours of continuous operation, the positioning deviation can reach ±25cm, which is far beyond the ±5cm accuracy requirement for orchard weeding / fertilizing.

[0016] (III) The contradiction between functional completeness and operational practicality: lack of handling of special scenarios

[0017] 1. The emergency response mechanism after UWB signal loss is inadequate.

[0018] When the robot travels to the edge of the orchard or a blind area with dense fruit trees, the UWB signal is easily lost. Existing solutions often switch to pure vision SLAM mode, but the repetitive orchard scene can cause the SLAM loop closure detection failure rate to exceed 40%, which in turn causes the robot to veer off course.

[0019] 2. Conflict between obstacle identification and inter-row passage.

[0020] Most solutions treat fruit tree branches and weeds as non-collision obstacles and use the traditional A* algorithm to generate detour paths. This results in the robot's actual passage width increasing from 1.2m to 2.0m, making it unsuitable for the standard 1.5m row spacing in orchards and reducing work efficiency by more than 30%.

[0021] Therefore, there is an urgent need to propose a new navigation system and method to solve the problems existing in the current technology. Summary of the Invention

[0022] The technical problem to be solved by this invention is to provide a UWB navigation system and method based on visual feature fusion, which utilizes visual semantic information to calibrate UWB navigation and positioning results, achieving deep fusion of UWB and visual data. This solves the positioning limitations of single technologies in orchard environments, achieving centimeter-level positioning accuracy, improving the robot's adaptability in complex lighting and terrain environments, reducing hardware costs, and improving work efficiency and quality. It is suitable for high-precision navigation of orchard weeding robots in complex environments.

[0023] To solve the above-mentioned technical problems, the technical solution adopted by the present invention is as follows:

[0024] A UWB navigation method based on visual feature fusion is applied to a UWB navigation system based on visual feature fusion. The system includes a UWB positioning module, a visual perception module, a data fusion processing module, and a navigation control module. The UWB positioning module includes UWB anchor points deployed in an orchard and UWB tags set in a weeding robot. The visual perception module, data fusion processing module, and navigation control module are all set in the weeding robot, which is also equipped with a soil sensor. The method includes the following steps:

[0025] The layout of adaptively generated UWB anchor points is used to obtain the UWB coordinate library of anchor points;

[0026] The soil sensor collects the soil moisture in the orchard in real time, while the visual perception module collects orchard images in real time and identifies fruit trees and obstacles. The fruit tree coverage rate is calculated based on the fruit tree identification results.

[0027] The UWB positioning module calculates the UWB coordinates of the weeding robot in real time based on the anchor point UWB coordinate library and the UWB anchor point ToF signal received by the UWB tag. Then, it calibrates the UWB coordinates of the weeding robot according to the fruit tree coverage and soil moisture to obtain the UWB positioning data of the weeding robot.

[0028] The visual perception module obtains and corrects visual coordinates from the collected orchard images to obtain the visual positioning data of the weeding robot.

[0029] The data fusion processing module calculates the weights of UWB positioning data and visual positioning data, and then calculates the fused coordinates based on the UWB positioning data, visual positioning data and corresponding weights. After error correction of the fused coordinates, the optimal coordinates are obtained.

[0030] The navigation control module calculates the target path point based on the optimal coordinates, obstacle recognition results, and preset weeding operation path, and controls the weeding robot to travel to the target path point.

[0031] Furthermore, in the adaptive generation of UWB anchor point layouts, multiple populations are established. Different UWB anchor point layouts that satisfy the UWB anchor point parameter constraints are taken as individuals in each population. With the goal of minimizing the three-dimensional spatial geometric accuracy factor, selection, crossover, and mutation operations are iteratively performed on the individuals in each population to obtain the UWB anchor point layout corresponding to the optimal individual. The UWB anchor point parameter constraints include: the communication radius is a specified value, the installation height is the sum of the fruit tree height and a specified height, and the distance from the fruit tree trunk is greater than a distance threshold.

[0032] Furthermore, when calibrating the UWB coordinates of the weeding robot based on the fruit tree coverage and soil moisture, the following steps are included:

[0033] Calculate the multipath impact coefficient based on fruit tree coverage. And calculate the soil attenuation coefficient based on soil moisture. Multipath Influence Coefficient Soil attenuation coefficient Basic noise in open environment Multiplying them together yields the corrected noise covariance. ;

[0034] Based on the corrected noise covariance The Kalman filter algorithm is used to correct the UWB coordinates, as shown in the following formula:

[0035]

[0036]

[0037]

[0038] in, This represents the state prediction vector for the current frame. This represents the state vector of the previous frame. This represents the velocity vector of the weeding robot in the previous frame. Indicates the gain of the current frame. This represents the covariance matrix of the error between the estimated state (e.g., position, velocity) of the weeding robot and its true state after the current frame state prediction is completed and before the measurement update. Here is the state transition matrix. For the control matrix, For the observation matrix, This represents the state vector of the current frame. The x and y components of the state vector are the x and y coordinate values ​​in the corrected UWB coordinates. These are the UWB coordinate observations for the current frame.

[0039] Furthermore, the orchard image includes RGB images and near-infrared images. The weeding robot is also equipped with a light sensor for real-time acquisition of light values. When the visual perception module acquires orchard images in real time and identifies fruit trees and obstacles, it includes the following steps:

[0040] If the illumination value is greater than the illumination threshold, the current RGB image is converted to HSV space, and overexposed areas are filtered out using a preset V channel threshold. Then, the processed current RGB image and the current near-infrared image are input into the trained fruit tree detection model and obstacle detection model. Otherwise, the current RGB image and the current near-infrared image are input into the trained fruit tree detection model and obstacle detection model.

[0041] The fruit tree detection model extracts features from RGB and near-infrared images, then fuses these features using an attention mechanism. Based on the fused features, it identifies the fruit trees and finally obtains the trunk coordinates and calculates the center lines of the rows and columns. The obstacle detection model uses semantic segmentation on the RGB and near-infrared images to obtain an obstacle semantic map. The feature formula for fusing the RGB and near-infrared images using the attention mechanism is as follows:

[0042]

[0043] in, For the Sigmoid function, and Features of RGB and near-infrared images, respectively. and These are the fusion weights corresponding to the features of the RGB image and the near-infrared image, respectively.

[0044] Furthermore, when the visual perception module obtains and corrects visual coordinates from the acquired orchard images, it includes the following steps:

[0045] Select keyframes from each orchard image at specified intervals;

[0046] Calculate the deviation between the row and column spacing of fruit trees in the current frame of the orchard image and the row and column spacing of fruit trees in the corresponding key frame. If the deviation is less than the deviation threshold, calculate the difference between the visual coordinates of the current frame of the orchard image and the corresponding key frame to obtain the error correction amount. Use the error correction amount to correct the visual coordinates of the current frame of the orchard image and the subsequent frame of the orchard image.

[0047] Furthermore, when the data fusion processing module calculates the weights of UWB positioning data and visual positioning data, it includes the following steps:

[0048] The formulas for calculating the UWB evaluation factor and the visual evaluation factor are as follows:

[0049]

[0050]

[0051] in, As a UWB evaluation factor, For visual assessment factors, It is the signal strength of the communication between the UWB tag and the UWB anchor point in the current frame. It is the fruit tree coverage rate in the current frame. It is the soil attenuation coefficient corresponding to the soil moisture in the current frame. It is the illumination interference factor corresponding to the illumination value of the current frame. It is the weed occlusion factor corresponding to the obstacle recognition result of the current frame. It is the matching rate between the orchard image in the current frame and the corresponding keyframe, calculated using the following formula:

[0052]

[0053] Specifically, the number of matching points is the total number of feature points that match the feature point set F of the current frame orchard image with the feature point set F of the corresponding key frame, and the total number of extracted points is the total number of feature points in the feature point set F of the previous frame orchard image.

[0054] The weights of UWB positioning data and visual positioning data are calculated based on UWB evaluation factors and visual evaluation factors, using the following formula:

[0055]

[0056]

[0057] in, Indicates the weight of UWB positioning data. This indicates the weight of the visual positioning data.

[0058] Furthermore, when calculating the fused coordinates based on UWB positioning data, visual positioning data, and corresponding weights, a Kalman filter algorithm is specifically used to calculate the fused coordinates based on the UWB positioning data, visual positioning data, and corresponding weights, as shown in the following formula:

[0059]

[0060]

[0061]

[0062] in, This represents the state prediction vector for the current frame. This represents the state vector of the previous frame. This represents the velocity vector of the weeding robot in the previous frame. This represents the x-coordinate value in the UWB coordinates of the UWB positioning data. This represents the x-coordinate value in the visual positioning coordinates of the visual positioning data. Indicates the merged coordinates of the previous frame. Indicates the gain of the current frame. This represents the covariance matrix of the error between the estimated state (e.g., position, velocity) of the weeding robot and its true state after the current frame state prediction is completed and before the measurement update. Here is the state transition matrix. For the control matrix, For the observation matrix, This represents the state vector of the current frame. The x and y components of the state vector are the values ​​of the x and y coordinates in the fused coordinate system. The fused coordinate observation value for the current frame is given by the following formula:

[0063]

[0064] in, This represents UWB positioning data. This represents visual positioning data.

[0065] Furthermore, when correcting for errors in the fused coordinates, the following steps are included:

[0066] Calculate the vertical distance from the fused coordinates to the center line of the corresponding fruit tree row and column as the distance deviation. Calculate the distance from the fused coordinates to the nearest tree trunk in the orchard image. and according to the distance Calculate spacing deviation The formula is as follows:

[0067]

[0068]

[0069] in, and These are the x and y coordinates from the merged coordinate system, respectively. Indicates the slope of the center line of the fruit tree rows. The intercept of the center line of the fruit tree row and column, This indicates the preset standard spacing between fruit trees;

[0070] If distance deviation Spacing deviation If all values ​​are less than the corresponding threshold, the fused coordinates are taken as the optimal coordinates; otherwise, particle filter resampling is initiated to correct the fused coordinates and obtain the corresponding optimal coordinates.

[0071] Furthermore, the navigation control module calculates the target path point based on the optimal coordinates, obstacle recognition results, and the preset weeding operation path, and controls the weeding robot to travel to the target path point, including the following steps:

[0072] The difference between the optimal coordinates of the current frame and the coordinates of the corresponding reference path point in the weeding operation path is calculated to obtain the path deviation. A cost map is constructed based on the obstacle recognition results. In the cost map, if no obstacle is recognized at the current position, the cost value is the minimum value; if an obstacle is recognized at the current position and the obstacle recognition result shows that the type is flexible or the diameter is less than the diameter threshold, the cost value is the intermediate value; if an obstacle is recognized at the current position and the obstacle recognition result shows that the type is rigid or the diameter is greater than the diameter threshold, the cost value is the maximum value.

[0073] Using the optimal coordinates of the current frame as the starting point, calculate the heuristic function and cost function of the neighboring nodes around the starting point, and update the cost function using the cost value of the neighboring nodes in the cost map. Select the neighboring node with the smallest sum of the heuristic function and cost function as the target path point. The formula for the heuristic function is as follows:

[0074]

[0075] in, and These represent the coordinates of the current point. and These are the x and y coordinates of the target path point, respectively. The mean y-axis value of the center lines of the rows and columns of fruit trees in the fruit tree identification results;

[0076] To predict the future tilt angle, obtain the slope angle of the weeding robot's location and its current left and right wheel speeds, using the following formula:

[0077]

[0078] in, The slope angle, These are the current left wheel speed and right wheel speed, respectively.

[0079] The PID parameters are corrected based on the slope angle and future tilt angle. Then, the PID controller with corrected parameters is used to calculate the motion control quantity of the weeding robot in the current frame, as shown in the following formula:

[0080]

[0081]

[0082] in, For linear velocity in motion control quantities, Angular velocity is a variable in motion control. For the current actual linear velocity, The current actual angular velocity, , , These are the PID parameters for linear velocity; , , Angular velocity PID parameters; , The linear velocity and angular velocity deviations from the previous frame. , Let x be the linear velocity deviation and angular velocity deviation of the i-th frame. , The linear velocity deviation and angular velocity deviation for the current frame are calculated using the following formulas:

[0083]

[0084]

[0085] in, and These represent the x-direction position deviation and the y-direction position deviation in the path deviation, respectively. and These are the proportionality coefficients corresponding to the linear velocity deviation and the angular velocity deviation, respectively.

[0086] The desired left wheel speed and desired right wheel speed are calculated based on the linear velocity and linear velocity deviation in the motion control variables of the current frame of the weeding robot. The weeding robot is then controlled to travel to the target path point based on the desired left wheel speed and desired right wheel speed.

[0087] This invention also proposes a UWB navigation system based on visual feature fusion, including a UWB positioning module, a visual perception module, a data fusion processing module, and a navigation control module. The UWB positioning module includes UWB anchor points deployed in the orchard and UWB tags set in the weeding robot. The visual perception module, data fusion processing module, and navigation control module are all set in the weeding robot, which is also equipped with a soil sensor. The UWB navigation system based on visual feature fusion is used to execute the steps in the UWB navigation method based on visual feature fusion.

[0088] Compared with the prior art, the advantages of the present invention are as follows:

[0089] This invention forms a closed loop of "scene perception - dynamic fusion - intelligent adaptation": soil moisture and fruit tree coverage directly participate in the calibration of UWB positioning data and the correction of visual positioning data, so that the fused coordinates are adjusted in real time with changes in the orchard environment. The optimal coordinates obtained after error correction of the fused coordinates are input into the navigation control module, and together with the obstacle recognition results, they determine the target path point, completing real-time path planning and walking control for the orchard scene. The environmental features are transmitted layer by layer, completing the full-link layered processing from environmental collection, data fusion to driving adaptation.

[0090] This invention quantifies the degree of vegetation occlusion by identifying fruit tree coverage and quantifies changes in ground media by soil moisture. The two are used together for UWB coordinate calibration and visual coordinate correction, so that the positioning results are adjusted according to changes in orchard vegetation and soil conditions. The navigation control module combines obstacle recognition and preset operation paths to generate target points, adapting to specific spatial distributions such as tree trunks and grass in the orchard.

[0091] This invention utilizes both visually perceived fruit tree coverage and soil sensor-collected humidity data to simultaneously influence the UWB positioning module, enabling information injection from the perception layer to the positioning layer. The data fusion processing module outputs the weighted calculation results of the two types of positioning data to the navigation control module, allowing path point calculation to simultaneously rely on optimal coordinates and obstacle recognition results. This achieves cross-module data sharing and closed-loop optimization, realizing integrated linkage between positioning, planning, and control. It avoids information silos caused by independent calculations of single modules, significantly improving the navigation stability and operational continuity of the system in complex orchard environments. Attached Figure Description

[0092] Figure 1 This is a system architecture diagram of an embodiment of the present invention.

[0093] Figure 2 This is a simplified flowchart of the method according to an embodiment of the present invention.

[0094] Figure 3 A schematic diagram of the UWB positioning module in operation.

[0095] Figure 4 This is a flowchart of the visual perception module's workflow.

[0096] Figure 5 This is a flowchart of the data fusion processing module.

[0097] Figure 6 A detailed flowchart illustrating the operation of the navigation control module. Detailed Implementation

[0098] The present invention will be further described below with reference to the accompanying drawings and specific preferred embodiments, but this does not limit the scope of protection of the present invention.

[0099] Existing navigation methods for orchard weeding robots do not fully consider the complexity of the special environment of orchards, and still have shortcomings in terms of positioning accuracy, environmental adaptability, and reliability. Specifically:

[0100] Lack of dynamic environmental adaptation: Orchard environments change drastically with the seasons. The lush green leaves in spring and summer, the ripening fruit in autumn, and the bare branches in winter cause fundamental changes in visual features (such as leaf texture and color contrast). Existing solutions mostly use fixed-trained visual models or static maps, without establishing a parameter update mechanism that links with the seasons. For example, the YOLOv series fruit tree detection model trained in summer loses accuracy from 90% to 65% due to the fruit obscuring the trunk features in autumn; in winter, due to leaf drop, the number of feature points relied upon by visual SLAM decreases by 70%, resulting in a localization drift of more than ±20cm, which is completely unable to meet the requirements for stable operation throughout the year.

[0101] Inadequate handling of multi-interference source coupling: UWB signal interference and visual interference often occur simultaneously in orchards (e.g., during heavy rain, water accumulation on branches and leaves exacerbates the UWB multipath effect, while rainwater obscures the camera, leading to a decrease in visual matching rate). Existing solutions mostly handle single interference independently (e.g., only optimizing UWB filtering or adjusting the visual threshold separately), without establishing a "multi-interference source coupling assessment-cooperative compensation" mechanism, resulting in a sharp drop of more than 50% in fusion positioning accuracy under combined interference.

[0102] The existing solutions are disconnected from operational requirements and navigation logic: They only focus on the basic functions of "positioning and obstacle avoidance" and do not take into account the core requirements of orchard weeding operations (such as weed coverage of ≥95% between rows and avoiding damage to the root system of fruit trees). For example, traditional navigation path planning does not constrain the distance between the weeding device and the trunk of the fruit tree (which can easily lead to root damage due to path deviation), and it does not consider dynamically adjusting the path according to the density of weeds (it still travels at a fixed speed in areas with sparse weeds, resulting in energy waste), making it difficult to balance operational quality and efficiency.

[0103] Lack of long-term reliability assurance: Soil subsidence in orchards (annual subsidence of 3-5cm) and branch growth (quarterly extension of 10-15cm) will cause the preset anchor point layout and visual feature library to gradually become invalid. The existing solution lacks a regular calibration and update mechanism. After 6 months of operation, the UWB positioning error increases by 40% due to changes in the relative position of the anchor points. The visual map shows more than 30% feature mismatch due to branch growth, requiring manual redeployment or calibration, which greatly increases the operation and maintenance costs.

[0104] To address the aforementioned issues, this embodiment proposes a UWB navigation system based on visual feature fusion. This system fully leverages the absolute positioning accuracy of UWB and the perception advantages of visual environmental features, effectively overcoming the limitations of single technologies in orchard environments. It achieves centimeter-level positioning accuracy to meet the demands of high-precision operations. For complex lighting and terrain environments in orchards, the system dynamically adjusts the UWB and visual data fusion strategy, maintaining stable and reliable navigation performance in adverse lighting conditions such as nighttime, rain, strong sunlight, and areas with dense fruit trees and signal obstruction, significantly improving the robot's environmental adaptability. Compared to pure UWB positioning technology, it reduces the number of UWB anchor points deployed to lower hardware costs. Simultaneously, it utilizes visual navigation to achieve positioning in UWB signal blind spots, enhancing the system's adaptability to different orchard terrains and layouts, and increasing the robot's operational flexibility. Furthermore, accurate positioning and reliable navigation ensure that the robot efficiently completes weeding operations along a predetermined path, reducing missed and repeated weeding, improving operational efficiency and quality, reducing manual labor intensity, and promoting the improvement of intelligent orchard management.

[0105] like Figure 1 As shown, the system in this embodiment includes a UWB positioning module, a visual perception module, a data fusion processing module, and a navigation control module, wherein:

[0106] The UWB positioning module consists of multiple UWB anchor points deployed within the orchard and UWB tags installed on the weeding robot. The UWB anchor points utilize high-precision UWB positioning equipment, operating in the 3.1-10.6GHz frequency band, with a positioning accuracy of ±5cm. Depending on the orchard area and terrain, an anchor point is placed every 50-100 meters along the orchard boundary, and one anchor point is placed at the intersections of main paths and at both ends of each row of fruit trees. The anchor points are installed at a height of 2-3 meters to ensure effective signal coverage of the orchard area while minimizing the impact of tree shading. The anchor points are powered by solar panels and equipped with backup batteries to ensure stable operation over extended periods.

[0107] UWB tags integrate miniaturized UWB tags into the control system of weeding robots. The tags support multi-channel data reception and can communicate simultaneously with multiple anchor points to obtain distance information in real time. The tags feature a low-power design and are powered by the robot's power system.

[0108] The visual perception module is equipped with a binocular camera or an RGB-D camera, mounted at a suitable position at the front of the robot to ensure that it can acquire environmental images and depth information within a certain range in front. In this embodiment, the visual perception module includes a camera and an image processing unit.

[0109] The camera is a binocular camera with a resolution of 1920×1080, a frame rate of 30fps, a baseline distance of 12cm, and a field of view of 120°, capable of acquiring clear environmental images and depth information. The camera is mounted on the upper front of the robot, 1.2-1.5 meters above the ground, ensuring comprehensive capture of the environment within a 5-10 meter range in front. It is also equipped with an infrared auxiliary light that automatically activates at night or in low-light conditions to provide supplementary illumination for the camera.

[0110] The image processing unit uses a high-performance embedded NVIDIA Jetson AGXXavier processor as its core, possessing powerful computing capabilities and the ability to run complex computer vision algorithms in real time. The processor connects to the camera via a USB interface, receives image data, and transmits the processed results to the data fusion module.

[0111] The data fusion processing module uses Kalman filtering or particle filtering algorithms to fuse UWB positioning data and visual perception data. The navigation control module generates motion control commands for the robot based on the position information output by the data fusion processing module and the pre-planned weeding operation path.

[0112] In this embodiment, the data fusion processing module and navigation control module are built on an industrial-grade computer motherboard, equipped with an Intel Core i7 multi-core processor, 16GB of memory, and a high-speed solid-state drive, running a LinuxRT system for real-time operation. The data fusion processing module and navigation control module are connected to the UWB positioning module and visual perception module via a PCI-e high-speed data bus, enabling rapid data transmission and processing. Simultaneously, a 5G wireless communication module is integrated, facilitating data interaction and remote control between the robot and a remote monitoring center.

[0113] Furthermore, this embodiment also proposes a UWB navigation method based on visual feature fusion, which is applied to the aforementioned UWB navigation system based on visual feature fusion, such as... Figure 2 As shown, the method includes the following steps:

[0114] First, perform system initialization, including the following steps:

[0115] S0) Adaptively generate the layout of UWB anchor points to obtain the anchor point UWB coordinate library, thereby establishing the orchard global coordinate system; at the same time, start the weeding robot. At this time, the UWB tag obtains the initial position information, and the visual perception module collects images of the robot's surrounding environment, identifies feature points such as fruit trees, constructs a local feature map, and associates and calibrates it with the initial position obtained by the UWB tag to complete the system initialization.

[0116] Next, real-time positioning and data collection are performed, including the following steps:

[0117] S1) The soil sensor installed in the weeding robot collects the soil moisture of the orchard in real time. At the same time, the vision perception module collects orchard images in real time and identifies fruit trees and obstacles. The fruit tree coverage is calculated based on the fruit tree identification results. The UWB positioning module calculates the UWB coordinates of the weeding robot in real time based on the anchor point UWB coordinate library and the UWB anchor point ToF signal received by the UWB tag. Then, the UWB coordinates of the weeding robot are calibrated according to the fruit tree coverage and soil moisture to obtain the UWB positioning data of the weeding robot.

[0118] S2) The visual perception module obtains and corrects the visual coordinates from the collected orchard images to obtain the visual positioning data of the weeding robot. When correcting the visual coordinates, the visual loop closure detection technology is used to correct the cumulative error of visual positioning to ensure the accuracy and reliability of the fusion results.

[0119] Through the above steps, during the operation of the weeding robot, the UWB positioning module continuously acquires the robot's position coordinates; the visual perception module collects environmental images at a certain frequency, performs fruit tree recognition, obstacle detection and feature point extraction, and obtains visual positioning information and environmental perception data.

[0120] The data fusion process is performed again, including the following steps:

[0121] S3) Input UWB positioning data and visual positioning information into the data fusion processing module. The data fusion processing module calculates the weights of UWB positioning data and visual positioning data according to the current environmental conditions. Then, it calculates the fused coordinates based on the UWB positioning data, visual positioning data and corresponding weights. During the fusion process, the module cross-validates the positioning data with geometric features such as the spacing between rows and columns of fruit trees and the relative positional relationship of tree trunks to identify and remove outliers in the fused coordinate data. After error correction, the optimal coordinates are obtained.

[0122] Finally, path planning and navigation control are performed, including the following steps:

[0123] S4) The navigation control module calculates the target path point based on the optimal coordinates, obstacle recognition results, and the preset weeding operation path, and controls the weeding robot to travel to the target path point. During this process, the navigation control module calculates the deviation between the fused position information and the preset weeding operation path, and controls the robot to safely bypass obstacles and return to the predetermined operation path to perform the weeding task based on environmental perception data and the deviation.

[0124] The following is a detailed explanation of each step.

[0125] In step S0 of this embodiment, when adaptively generating the layout of UWB anchor points, the three-dimensional coordinates of the anchor points are optimized based on a multi-population genetic algorithm (MPGA) combined with the orchard micro-topography and fruit tree distribution characteristics. This solves the problem of uneven three-dimensional spatial positioning accuracy caused by the conventional use of equally spaced two-dimensional planar deployment of anchor points (e.g., one every 50 meters), which does not consider the orchard terrain undulations (slope 5°-15°) and fruit tree height (2.5-3.5m). The specific steps are as follows:

[0126] Set constraints, specifically the basic parameters of the orchard: orchard boundary dimensions (length L × width W), and terrain slope. (Pre-collection by dual-axis tilt sensors on the chassis of the weeding robot, range 5°-15°), spacing between rows of fruit trees (Vision module pre-detection, 3-4m); UWB anchor point parameter constraints are: communication radius. Installation height constraints ( Constraints include the height of the fruit tree (2.5-3.5m) and the distance from the main trunk of the fruit tree. ;

[0127] Initializing the population involves creating multiple populations, with different UWB anchor point layouts that satisfy the UWB anchor point parameter constraints serving as individuals in each population. In this embodiment, 10 populations are generated, each containing 20 anchor point layout schemes. The above constraints are satisfied;

[0128] In this embodiment, the fitness calculation aims to minimize the three-dimensional spatial geometric precision factor. The formula for calculating the three-dimensional spatial geometric precision factor is as follows:

[0129]

[0130] Where G is the anchor point position matrix;

[0131] Iterative optimization specifically involves iteratively selecting, crossing over, and mutating individuals in each population. In this embodiment, the number of iterations is 50 generations, with a crossover probability of 0.8 and a mutation probability of 0.1.

[0132] After iterative optimization, the optimal UWB anchor point layout for each individual is obtained, and finally, the corresponding 3D coordinate library of the anchor points is output. .

[0133] The UWB anchor point layout obtained through the above steps incorporates three-dimensional parameters such as orchard terrain slope and tree height into the anchor point optimization model, reducing the number of anchor points by 30% compared to the conventional two-dimensional layout, and improving positioning accuracy by 40% in a 15° slope scenario.

[0134] In step S1 of this embodiment, the UWB positioning module employs a two-factor Kalman filter method based on fruit tree density and soil moisture. It constructs a dynamic error compensation model using real-time fruit tree coverage and soil sensor data collected by the vision module. This solves the problem of conventional UWB positioning using general TDoA / ToF algorithms, which are not optimized for scenarios such as orchard foliage shading and soil moisture fluctuations, resulting in significant multipath errors. It also overcomes the shortcomings of conventional Kalman filtering, which relies solely on signal strength to adjust gain and cannot distinguish the source of error, as UWB signal errors in orchards originate from multipath reflection from branches and leaves (positively correlated with fruit tree coverage) and soil moisture attenuation (negatively correlated with moisture). Specifically, the UWB positioning module calculates the UWB coordinates of the weeding robot in real-time based on the anchor point UWB coordinate library and the UWB anchor point ToF signals received by the UWB tags. Then, when calibrating the UWB coordinates of the weeding robot based on the fruit tree coverage and soil moisture, such as... Figure 3 As shown, it includes the following steps:

[0135] First, by acquiring raw UWB positioning data, the UWB coordinates of the weeding robot are calculated in real time based on the anchor point UWB coordinate library and the UWB anchor point ToF signal received by the UWB tag. The specific process is as follows:

[0136] Obtain the 3D coordinate library of anchor points And the ToF signals received by the UWB tag from 3 or more anchor points (Time of Flight, in nanoseconds), distance calculated based on the Time-of-Flight (ToF) algorithm. (c is the speed of light, i is the anchor point number);

[0137] By using the trilateration method and solving the simultaneous equations

[0138]

[0139] Solve for the robot's initial coordinates Finally, the original UWB distance set is output. with initial coordinates ;

[0140] Then, using multi-factor Kalman filtering, the UWB coordinates of the weeding robot are calibrated based on the fruit tree coverage and soil moisture. The specific process is as follows:

[0141] Calculate the multipath impact coefficient based on fruit tree coverage. The formula is:

[0142]

[0143] in, The fruit tree coverage rate is output in real time by the visual perception module;

[0144] The soil attenuation coefficient was calculated based on soil moisture. The formula is:

[0145]

[0146] in, The soil moisture is output in real time by the soil sensor;

[0147] Multipath Influence Coefficient Soil attenuation coefficient Basic noise in open environment Multiplying them together yields the corrected noise covariance. , In this embodiment, the basic noise level in an open environment The value is 0.01, which can be adjusted according to the actual situation;

[0148] Based on the corrected noise covariance The Kalman filter algorithm is used to correct the UWB coordinates, as shown in the following formula:

[0149]

[0150]

[0151]

[0152] in, This represents the state prediction vector for the current frame. This represents the state vector of the previous frame. This represents the velocity vector of the weeding robot in the previous frame. Indicates the gain of the current frame. Let represent the covariance matrix of the error between the estimated state (e.g., position, velocity) of the weeding robot and the actual state after the state prediction is completed in the k-th frame (current frame) and before the measurement update. Here is the state transition matrix. For the control matrix, For the observation matrix, This represents the state vector of the current frame. The x and y components of the state vector are the x and y coordinate values ​​in the corrected UWB coordinates. These are the UWB coordinate observations for the current frame.

[0153] Through the above steps, the environmental parameters perceived visually are deeply coupled with UWB filtering, overcoming the limitations of conventional filtering that relies solely on the characteristics of the signal itself. In scenarios with fruit tree coverage exceeding 70%, the positioning error is reduced from ±30cm to within ±8cm. The final output is the corrected UWB coordinates. Compare it with the corresponding UWB signal strength (Unit: dBm) Transmitted to the data fusion processing module.

[0154] In step S1 of this embodiment, the visual perception module fuses near-infrared (850nm) and RGB images, designs a dynamic threshold detection mechanism, and utilizes the geometric features of fixed row spacing (3-4m) and stable trunk diameter (15-20cm) in the orchard to construct scene-constrained assisted closed-loop detection. This effectively solves the problem that conventional visual perception algorithms using fixed parameters like YOLO / SLAM cannot handle issues such as sudden changes in orchard lighting, weed occlusion, and dynamic changes in tree growth. Figure 4 As shown, when the visual perception module acquires orchard images in real time and identifies fruit trees and obstacles, it includes the following steps:

[0155] Image preprocessing:

[0156] Acquire RGB images captured in real time by binocular cameras (Resolution 1920×1080), near-infrared image (wavelength 850nm), real-time illumination value L (unit: lux) output by the camera light sensor of the weeding robot.

[0157] If the illumination value is greater than the illumination threshold, the current RGB image is converted to HSV space, and overexposed areas are filtered through a preset V channel threshold; otherwise, the current RGB image is not processed. In this embodiment, the illumination threshold is 8000 lux.

[0158] Multispectral feature fusion:

[0159] In this embodiment, the fruit tree detection model uses the YOLOv12 model, and a near-infrared feature channel is added to the YOLOv12 backbone. After the target detection model extracts features from the RGB and near-infrared images, it fuses the features of the RGB and near-infrared images through an attention mechanism, and then identifies fruit trees based on the fused features. The feature formula for fusing the RGB and near-infrared images through the attention mechanism is as follows:

[0160]

[0161] in, For the Sigmoid function, and Features of RGB and near-infrared images, respectively. and These are the fusion weights corresponding to the features of the RGB image and the near-infrared image, respectively;

[0162] In this embodiment, the obstacle detection model uses the PSPNet semantic segmentation model to perform semantic segmentation on RGB and near-infrared images, outputting the obstacle type (rigid / flexible) and diameter. ,coordinate

[0163] Object detection:

[0164] Input the current RGB image and the current near-infrared image into the trained fruit tree detection model and obstacle detection model to obtain the recognition results of fruit trees and obstacles;

[0165] For fruit trees identified by the fruit tree detection model, the trunk coordinates of the identified fruit trees are obtained and the center lines of the rows and columns of the fruit trees are calculated. Specifically, this involves analyzing the set of trunk coordinates in the orchard image. The row and column center lines were fitted using the least squares method. The formula for the center line of the row and column is as follows:

[0166]

[0167] in, Indicates the slope of the center line of the fruit tree rows. It represents the intercept of the center line of the fruit tree row.

[0168] For obstacles identified by the obstacle detection model, construct an obstacle semantic map. , , )}.

[0169] The final output is the center line of the fruit tree row and column. The tree trunk coordinate set T is used as the fruit tree recognition result, and an obstacle semantic map is output. , , )}, as the result of obstacle recognition.

[0170] Through the above steps, the fruit tree detection model combines multispectral features with dynamic thresholds, solving the problem of visual misjudgment in orchards caused by strong light saturation, shadow interference, confusion between weeds and fruit tree features, and the fact that conventional YOLO relies on RGB images and does not utilize the spectral characteristics of vegetation, resulting in 30% mismatch of feature points in strong light / shadow scenes. The misjudgment rate of weeds has been reduced from 50% to below 8%.

[0171] In step S1 of this embodiment, when calculating the fruit tree coverage rate based on the fruit tree recognition results, the fruit tree coverage rate can be calculated based on the proportion of the tree trunk coordinate set T in the orchard image or the proportion of the bounding box of the fruit tree recognized by the fruit tree detection model in the orchard image. The relevant calculation formulas are well known to those skilled in the art and will not be elaborated here.

[0172] In step S2 of this embodiment, when the visual perception module obtains and corrects visual coordinates from the acquired orchard image, it utilizes the geometric features of the orchard's fixed row spacing (3-4m) and stable trunk diameter (15-20cm) to construct scene constraint-assisted loop closure detection. This addresses the problem of high loop closure detection failure rate in conventional SLAM within repetitive orchard scenes (such as rows of fruit trees with consistent row spacing). Figure 4 As shown, when the visual perception module obtains and corrects visual coordinates from the acquired orchard image, it performs the following steps for the preprocessed orchard image in step S1:

[0173] Input data:

[0174] Obtain the preprocessed current frame orchard image and the corresponding fruit tree recognition results. (T), and the visual coordinates of the previous frame. Obtaining visual coordinates from each frame of an image is common knowledge in the SLAM field. Generated through an iterative mechanism of "previous coordinates + feature motion increment + fruit tree verification", and is consistent with the current frame. A closed-loop computing link is formed to ensure the continuity and consistency of visual positioning.

[0175] Feature point extraction:

[0176] The ORB algorithm is used to extract stable features such as tree trunk edges and branch points from the current frame of the orchard image, and leaf features are filtered out to obtain a new set of feature points F. The z-component can be derived from the depth information in the orchard image. Obtain;

[0177] Feature matching:

[0178] The LoFTR algorithm is used to calculate the matching rate between the current frame of the orchard image and the corresponding keyframe. In this embodiment, keyframes are selected from each frame of the orchard image at specified intervals. Specifically, the weeding robot extracts a keyframe every 5 meters it travels. Each extracted keyframe corresponds to all orchard images collected within the 5 meters it travels. For example, the keyframe extracted at 5 meters corresponds to all orchard images collected from 0 meters to 5 meters, the keyframe extracted at 10 meters corresponds to all orchard images collected from 5 meters to 10 meters, and so on. The formula for calculating the matching rate is as follows:

[0179]

[0180] Among them, the number of matching points is specifically the total number of feature points that match in the set of feature points F of the current-frame orchard image and the set of feature points F of the corresponding key frame, and the total number of extracted points is specifically the total number of feature points in the set of feature points F of the previous-frame orchard image;

[0181] Local map construction:

[0182] Generate a corresponding local point cloud map according to the set of trunk coordinates T and the set of feature points F corresponding to the current-frame orchard image, and associate the local point cloud map with the visual coordinates , thereby constructing a local map;

[0183] Loop closure detection:

[0184] Record the visual coordinates of the key frame and the fruit tree features, that is, the set of trunk coordinates ;

[0185] Calculate the deviation between the row and column spacing of the fruit trees in the current-frame orchard image and the row and column spacing of the fruit trees in the corresponding key frame , and the calculation formula is as follows:

[0186]

[0187] Among them, represents the vertical distance between the centerlines of two adjacent rows of fruit trees calculated by visual detection in the current frame (the k-th frame), represents the vertical distance between the centerlines of two adjacent rows of fruit trees in the same area calculated by visual detection in the stored key frame (the k'-th frame, (k'<k));

[0188] If the deviation is greater than the deviation threshold, it is determined as a false loop closure. If the deviation is less than the deviation threshold, it is a true loop closure. In this embodiment, the deviation threshold is , where is the deviation between the row and column spacing of the fruit trees in the current-frame orchard image and the row and column spacing of the fruit trees in the corresponding key frame;

[0189] In the case of a true loop closure, first calculate the error correction amount, and then use the error correction amount to correct the visual coordinates of the current-frame orchard image and subsequent-frame orchard images. Specifically, calculate the difference between the visual coordinates of the current-frame orchard image and the corresponding key frame to obtain the error correction amount, that is 、 , where , The x and y coordinates of the current orchard image are given in the uncorrected visual coordinates. Then, the visual coordinates of the current orchard image and subsequent orchard images are corrected according to the error correction amount. The corrected visual coordinates of the current orchard image and subsequent orchard images are: .

[0190] By using the above steps, the geometric features of orchard trees are used as constraints for SLAM loop closure detection. The loop closure failure rate in repeated scenarios is reduced from 40% to 12%, and the cumulative error is < ±5cm after 4 hours of continuous operation.

[0191] In step S3 of this embodiment, the data fusion processing module constructs a multi-dimensional environmental factor evaluation model and dynamically adjusts the fusion weights to solve the problem that conventional data fusion uses fixed weights or general dynamic weights, which do not consider the correlation between UWB and visual errors in orchards (such as the simultaneous impact of branch and leaf occlusion on the accuracy of both). Figure 5 As shown, step S3 specifically includes the following steps:

[0192] S31) Calculate the weights of UWB positioning data and visual positioning data to achieve fusion weight calculation:

[0193] Obtain UWB positioning data output by the UWB positioning module Signal strength Visual positioning data output by the vision module Matching rate and obtain fruit tree coverage rate Soil moisture H is used as the basis for calculating the light interference factor based on real-time light intensity L, and the weed shading factor is calculated based on obstacle identification results. The formula for the light interference factor is:

[0194]

[0195] Weed shading factor Obstacle semantic map obtained from semantic segmentation , , If a result of type "weed" exists, its value is 1; otherwise, its value is 0.

[0196] The formulas for calculating the UWB evaluation factor and the visual evaluation factor are as follows:

[0197]

[0198]

[0199] in, As a UWB evaluation factor, For visual assessment factors, It is the signal strength of the communication between the UWB tag and the UWB anchor point in the current frame. It is the fruit tree coverage rate in the current frame. It is the soil attenuation coefficient corresponding to the soil moisture in the current frame. It is the illumination interference factor corresponding to the illumination value of the current frame. It is the weed occlusion factor corresponding to the obstacle recognition result of the current frame. It is the matching rate between the orchard image in the current frame and the corresponding keyframe;

[0200] The weights of UWB positioning data and visual positioning data are calculated based on UWB evaluation factors and visual evaluation factors, using the following formula:

[0201]

[0202]

[0203] in, Indicates the weight of UWB positioning data. This indicates the weight of the visual positioning data.

[0204] S32) Using adaptive Kalman filtering, the fused coordinates are calculated based on UWB positioning data, visual positioning data, and corresponding weights:

[0205] Obtain fusion weights ), UWB positioning module output The output of the visual perception module The robot motion control quantities output by the navigation control module Robot motion control quantity This represents the control input for the k-th frame, typically a velocity vector. ,in linear velocity, Angular velocity;

[0206] The Kalman filter algorithm is used to calculate the fused coordinates based on UWB positioning data, visual positioning data, and corresponding weights, as shown in the following formula:

[0207]

[0208]

[0209]

[0210] in, This represents the state prediction vector for the current frame. This represents the state vector of the previous frame. This represents the velocity vector of the weeding robot in the previous frame. This represents the x-coordinate value in the UWB coordinates of the UWB positioning data. This represents the x-coordinate value in the visual positioning coordinates of the visual positioning data. Indicates the merged coordinates of the previous frame. Indicates the gain of the current frame. This represents the covariance matrix of the error between the estimated robot state (e.g., position, velocity) and the true state after the state prediction is completed in the k-th frame (current frame) and before the measurement update. Here is the state transition matrix. For the control matrix, For the observation matrix, This represents the state vector of the current frame. The x and y components of the state vector are the values ​​of the x and y coordinates in the fused coordinate system. The fused coordinate observation value for the current frame is given by the following formula:

[0211]

[0212] in, This represents UWB positioning data. Represents visual positioning data;

[0213] Finally, the preliminary fusion coordinates are output. , State vector The x and y components;

[0214] S33) Error correction is performed on the fused coordinates through cross-validation of the fruit tree set:

[0215] Obtain preliminary fusion coordinates The row and column center lines in the corresponding fruit tree identification results The tree trunk coordinate set T, and the pre-set standard spacing of fruit trees in the orchard. ;

[0216] Calculate the vertical distance from the fused coordinates to the center line of the corresponding fruit tree row and column as the distance deviation. Calculate the distance from the fused coordinates to the nearest tree trunk in the orchard image. and according to the distance Calculate spacing deviation The formula is as follows:

[0217]

[0218]

[0219] in, and These are the x and y coordinates from the merged coordinate system, respectively. Indicates the slope of the center line of the fruit tree rows. The intercept of the center line of the fruit tree row and column, This indicates the preset standard spacing between fruit trees;

[0220] If distance deviation Spacing deviation If all values ​​are less than the corresponding threshold, then the fused coordinates are taken as the optimal coordinates. In this embodiment, the optimal coordinates are expressed as... ,like and ,but ;

[0221] Otherwise, initiate particle filtering resampling with 500 particles, remove outliers, and output the optimal coordinates.

[0222] The input data for particle filter resampling is the output of the adaptive Kalman filter. Fruit tree constraint parameters: row and column center lines Standard spacing The three most recent main coordinates (Visual module output), historical data, first 5 frames UWB correction coordinates { }, visual coordinates { Motion parameters: Control values ​​from the previous frame (Output from navigation control module).

[0223] The specific steps are as follows:

[0224] Particle initialization: Centered on Gaussian distribution Generate 500 particles, initial weights ;

[0225] Motion prediction: By Update particle positions;

[0226] Weight update: calculate particles to distance Deviation in distance from the main trunk Weighting formula: Normalization yields ;

[0227] Low variance resampling: Generate a random initial value r, select 500 high-weight particles by weighted summation, and reset the weights to 1 / 500;

[0228] Optimal coordinate output: Calculate the mean of the resampled particle set to obtain... ,verify , (If the condition is not met, iterate 3 times).

[0229] The above steps establish a correlation evaluation model between UWB and visual errors through multi-dimensional environmental perception-cross-validation fusion, which solves the problem of response lag in conventional fusion when the environment changes suddenly. The fusion latency is reduced from 200ms to 80ms, meeting the robot's operation speed requirement of 0.5m / s.

[0230] In step S4 of this embodiment, the navigation control module adopts flexible obstacle avoidance-attitude adaptive cooperative control, which solves the problem that conventional navigation control uses a general A* algorithm and PID control, failing to consider the micro-terrain undulations of the orchard, the malleability of branches, and the requirements of operational efficiency. On the one hand, by combining the obstacle diameter and material characteristics (branches can be bent) detected by vision, a flexible obstacle avoidance strategy is designed to solve the problem that conventional A* treats all obstacles as non-collision, causing the robot to be unable to pass through orchards with a row spacing of 1.5m (requiring a width of 2.0m). On the other hand, by combining the data from the dual-axis tilt sensor, an attitude prediction-PID cooperative control model is constructed, solving the problem that conventional PID only adjusts based on position deviation, failing to consider the tilting of the robot body caused by the undulations of the orchard terrain, which easily leads to sideslip. Figure 6 As shown, step S4 specifically includes the following steps:

[0231] S41) Path planning:

[0232] Obtain the optimal coordinates The obstacle semantic map in the obstacle recognition results output by the visual perception module Preset weeding operation path ;

[0233] The path deviation is obtained by calculating the difference between the optimal coordinates of the current frame and the coordinates of the corresponding reference path point in the weeding operation path. , ,in, and These represent the path deviations for the x and y coordinates, respectively. and These are the current reference path points corresponding to the optimal coordinates in the current frame within the weeding operation path. The values ​​of the x and y coordinates;

[0234] A cost map is constructed based on obstacle recognition results. In this cost map, if no obstacle is recognized at the current location, the cost value is the minimum; if an obstacle is recognized at the current location, and the obstacle recognition result indicates a flexible type or a diameter less than a diameter threshold, the cost value is the intermediate value; if an obstacle is recognized at the current location, and the obstacle recognition result indicates a rigid type or a diameter greater than a diameter threshold, the cost value is the maximum value. In this embodiment, by constructing a cost map, the environment is divided into feasible zones (…). ), flexible barrier area ( , ), rigid barrier zone ( , (), grid size 20cm × 20cm;

[0235] Based on A* algorithm ( Using the optimal coordinates of the current frame as the starting point, the heuristic function of the neighboring nodes around the starting point is calculated. With cost function And use the cost of neighboring nodes in the cost map Update cost function Selecting a heuristic function With cost function The node with the smallest sum and its neighbor is used as the target path point. It then generates corresponding obstacle avoidance commands. The formula for the heuristic function is as follows:

[0236]

[0237] in, and These represent the coordinates of the current frame. and These are the x and y coordinates of the target path point, respectively. Center lines of rows and columns of fruit trees in the fruit tree identification results The y-axis mean;

[0238] S42) Self-tuning PID control to obtain robot motion control quantities. :

[0239] Obtain obstacle avoidance instructions and obtain the target waypoint. Path deviation and And obtain the slope angle of the location output by the dual-axis tilt sensor of the weeding robot chassis. And the current left and right wheel speeds output by the wheel speed encoder; robot motion control quantities The calculations rely on the outputs of the preceding navigation control module and the data fusion module. The core inputs include:

[0240] The optimal location coordinates output by the data fusion module: (Current actual position of the robot);

[0241] Coordinates of the reference points for the preset work path: (The target location that the robot should reach);

[0242] Path deviation output by the navigation control module: (x-direction position deviation) (Position deviation in the y-direction);

[0243] The current motion state fed back by the wheel speed encoder: (Current actual linear velocity) (Current actual angular velocity);

[0244] Robot motion control quantity It is generated through two steps: "deviation calculation → PID control quantity solution", as detailed below:

[0245] First, calculate the speed deviation based on the path deviation, and then convert the position deviation into the deviation between the desired speed and the actual speed, as shown in the following formula:

[0246]

[0247]

[0248] in, This represents the linear velocity deviation between the desired velocity and the actual velocity. This represents the angular velocity deviation between the desired velocity and the actual velocity. and These are the proportionality coefficients corresponding to the linear velocity deviation and the angular velocity deviation, respectively. =0.8, =1.2;

[0249] Then, the PID parameters are calibrated, including:

[0250] Based on the kinematic model, the roll angle in the next 0.5 seconds is predicted using the following formula:

[0251]

[0252] in, These are the current left wheel speed and right wheel speed, respectively.

[0253] The PID parameters are corrected based on the slope angle and the future roll angle, specifically if... If the value is greater than 10°, the values ​​of the relevant PID parameters will be increased according to the preset rules, such as increasing the proportional coefficient. Increased from 2.0 to 3.0; if >5°, adjust the linear velocity deviation as follows ;

[0254] Finally, the parameter-corrected PID controller is used to calculate the motion control quantities of the robot in the current frame. ,include:

[0255] First, an incremental PID algorithm is used to calculate the motion control quantity based on the corrected PID parameters. The calculation formula is as follows:

[0256]

[0257]

[0258] in: =0.5、 =0.1、 =0.2 is the linear velocity PID parameter; =0.6、 =0.05、 =0.3 is the angular velocity PID parameter; , These represent the linear velocity and angular velocity deviations from the previous frame.

[0259] Ultimately, motor control quantity Represented as:

[0260] According to motor control volume The desired wheel speed is calculated by considering the linear velocity and its deviation, using the following formula:

[0261]

[0262]

[0263] in, and These are the expected left wheel speed and the expected right wheel speed, respectively. For motor control quantity linear velocity in This refers to the deviation in linear velocity.

[0264] Based on the desired left wheel speed and desired right wheel speed, output motor wheel speed command ( , ), control the weeding robot to travel to the target path point This completes the navigation loop.

[0265] Through the above steps, the flexibility of orchard obstacles is incorporated into path planning, reducing the passage width from 2.0m to 1.3m, increasing work efficiency by 25%, and reducing branch damage rate from 15% to 3%. At the same time, by combining terrain attitude prediction with PID control, the path deviation in a 15° slope scenario is reduced from ±15cm to ±3cm, and the climbing gradient is increased to 25%, meeting the needs of hilly orchards.

[0266] The method in this embodiment further includes the following steps:

[0267] System Testing and Optimization: The UWB navigation system was tested in a real orchard environment. Multiple repeated trials were conducted under various orchard terrains (flat ground, slopes, etc.) and lighting conditions (daytime, nighttime, cloudy, rainy days), recording performance metrics such as the robot's positioning accuracy, path tracking error, and obstacle avoidance success rate. Specific updated data is as follows:

[0268] Anchor point calibration: Every quarter, the visual perception module captures images of anchor point markers and combines them with optimal coordinates. Calculate the anchor point displacement. If the anchor point displacement exceeds ±3cm, proceed to step S0 to update the anchor point 3D coordinate library. ;

[0269] Visual parameter update: Compare the fruit tree feature deviation between new and old orchard images every month. If the fruit tree feature deviation exceeds ±10%, update the YOLOv12 parameters and SLAM map.

[0270] Real-time iteration: Repeat steps S1 to S4 of the above process every 10ms to ensure the real-time performance and accuracy of positioning and navigation.

[0271] Based on the test results, the UWB anchor point layout, visual algorithm parameters, and data fusion weight calculation function were optimized and adjusted to further improve the system's navigation performance and stability, ensuring that the system can meet the actual operational needs of the orchard weeding robot.

[0272] Special Case Handling: When the UWB signal is lost due to occlusion of the causal tree or other reasons, or when the signal quality is poor, the system automatically switches to a vision-driven loosely coupled mode, using a visual SLAM algorithm to maintain the robot's localization. Once the UWB signal is restored, the fusion of UWB and visual data is performed again to restore high-precision navigation. Simultaneously, in nighttime or low-light environments, the visual perception module activates infrared illumination and adjusts image algorithm parameters to ensure the effectiveness of visual navigation.

[0273] In summary, the UWB navigation system and method based on visual feature fusion proposed in this invention presents an innovative three-layer architecture of "scene perception - dynamic fusion - intelligent adaptation." Its core technologies feature "orchard-specific design" and "cross-module collaborative optimization," offering the following advantages:

[0274] (i) Orchard-specific dynamic fusion mechanism: environmental parameter-driven weight self-optimization

[0275] 1. Filtering algorithm triggered by multi-dimensional environmental perception

[0276] To address the UWB multipath effect, a dual-factor Kalman filter based on tree density and soil moisture was designed. This filter dynamically adjusts the filter gain (e.g., when the coverage is >70%) by real-time detection of tree coverage and soil moisture sensor data, reducing the gain from 0.8 to 0.3. This stabilizes the UWB positioning accuracy within ±8cm, reduces the anchor point deployment density from 50 meters to 80 meters, and lowers hardware costs by 25%.

[0277] To address visual occlusion: a weed-fruit tree feature differentiation network was developed—a “spectral feature attention module” was inserted into the YOLOv12backbone. By utilizing the difference in near-infrared reflectance between fruit trees and weeds, the misclassification rate of weeds was reduced from 30% to below 5%. At the same time, dynamic adjustment of infrared supplementary lighting was introduced to solve the problem of strong light saturation.

[0278] 2. Dynamic weight allocation in both spatiotemporal dimensions

[0279] Time dimension: Set environmental change response threshold - when the UWB signal strength fluctuates by more than 10dBm / second or the visual matching degree drops by more than 20% / second, trigger an emergency adjustment of weights to ensure that the fusion positioning delay is less than 100ms.

[0280] Spatial dimension: Establish an orchard scene weight mapping table—for example, when working between rows, the visual weight accounts for 60%-70%; in the turning area, the UWB weight accounts for 70%-80%, avoiding accuracy fluctuations caused by general weights.

[0281] (II) Adaptive calibration of coordinate system and terrain: micro-topography compensation + growth dynamic correction

[0282] 1. Real-time calibration of the coordinate system for micro-terrain sensing

[0283] By installing dual-axis tilt sensors and wheel speed encoders on the robot chassis, slope and travel distance data are collected in real time. Slope compensation and settlement correction are performed on the UWB and visual coordinate system, so that the cumulative error after 4 hours of continuous operation is controlled within ±5cm, which is 80% lower than the existing solution.

[0284] 2. Update of visual parameters for fruit tree growth dynamics

[0285] Establish a quarterly visual parameter database—based on the fruit tree growth cycle, four sets of visual algorithm parameters are preset. The robot automatically collects orchard images once a month and compares feature deviations through edge calculation. If the deviation exceeds ±10%, the parameters are updated to ensure that the positioning accuracy remains stable within ±8cm throughout the year, thus solving the "parameter fixation" defect of the existing solution.

[0286] (III) Emergency and Adaptation Solutions for Special Scenarios: Signal Loss Redundancy + Flexible Obstacle Avoidance

[0287] 1. Multi-level emergency response mechanism for UWB signal loss

[0288] Level 1 Emergency (Loss < 2 seconds): Enable rapid visual feature matching—pre-store visual templates of key locations in the orchard (such as intersections and fruit tree markers), and achieve relocation within 0.5 seconds with an accuracy of ±10cm through the ORB algorithm.

[0289] Level 2 emergency (loss of positioning for 2-5 seconds): Switch to visual-SLAM + inertial navigation fusion – using IMU sensors (accuracy ±0.01° / h) to compensate for SLAM drift, keeping the positioning deviation within ±15cm, improving stability by 50% compared to the pure SLAM solution.

[0290] 2. Path planning optimization for flexible obstacle avoidance

[0291] The improved A* algorithm introduces branch maneuverability judgment—by detecting the diameter and distance of obstacles through the vision module, a "semi-rigid path" is generated: when encountering branches with a diameter of 3-5cm, the robot is allowed to make slight collisions at an angle of less than 5°, reducing the passage width from 2.0m to 1.3m, adapting to the standard row spacing of orchards, and improving work efficiency by more than 25%.

[0292] Those skilled in the art will understand that embodiments of this application can be provided as methods, systems, or computer program products. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application can take the form of a computer program product embodied on one or more computer-readable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code. This application is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this application. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create a machine for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to operate in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1The functions specified in one or more boxes. These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable apparatus for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0293] The above description is merely a preferred embodiment of the present invention. The scope of protection of the present invention is not limited to the above embodiments. All technical solutions falling within the scope of the present invention's concept are within the scope of protection of the present invention. It should be noted that for those skilled in the art, any improvements and modifications made without departing from the principles of the present invention should also be considered within the scope of protection of the present invention.

Claims

1. A UWB navigation method based on visual feature fusion, applied to a UWB navigation system based on visual feature fusion, characterized in that, The system includes a UWB positioning module, a visual perception module, a data fusion processing module, and a navigation control module. The UWB positioning module includes UWB anchor points deployed in the orchard and UWB tags installed in the weeding robot. The visual perception module, data fusion processing module, and navigation control module are all installed in the weeding robot. The weeding robot is also equipped with a soil sensor. The method includes the following steps: The layout of adaptively generated UWB anchor points is used to obtain the UWB coordinate library of anchor points; The soil sensor collects the soil moisture in the orchard in real time, while the visual perception module collects orchard images in real time and identifies fruit trees and obstacles. The fruit tree coverage rate is calculated based on the fruit tree identification results. The UWB positioning module calculates the UWB coordinates of the weeding robot in real time based on the anchor point UWB coordinate library and the UWB anchor point ToF signal received from the UWB tag. Then, it calibrates the UWB coordinates of the weeding robot according to the fruit tree coverage and soil moisture to obtain the UWB positioning data of the weeding robot. The calibration of the UWB coordinates of the weeding robot according to the fruit tree coverage and soil moisture includes the following steps: Calculate the multipath impact coefficient based on fruit tree coverage. The formula is: in, The fruit tree coverage rate is output in real time by the visual perception module; The soil attenuation coefficient was calculated based on soil moisture. The formula is: in, The soil moisture is output in real time by the soil sensor; Multipath Influence Coefficient Soil attenuation coefficient Basic noise in open environment Multiplying them together yields the corrected noise covariance. , ; Based on the corrected noise covariance The Kalman filter algorithm is used to correct the UWB coordinates, as shown in the following formula: in, This represents the state prediction vector for the current frame. This represents the state vector of the previous frame. This represents the velocity vector of the weeding robot in the previous frame. Indicates the gain of the current frame. This represents the covariance matrix of the error between the estimated state of the weeding robot and its actual state after the current frame state prediction is completed and before the measurement update. Here is the state transition matrix. For the control matrix, For the observation matrix, This represents the state vector of the current frame. The x and y components of the state vector are the x and y coordinate values ​​in the corrected UWB coordinates. These are the UWB coordinate observations for the current frame; The visual perception module obtains and corrects visual coordinates from the collected orchard images to obtain the visual positioning data of the weeding robot. The data fusion processing module calculates the weights of UWB positioning data and visual positioning data, and then calculates the fused coordinates based on the UWB positioning data, visual positioning data and corresponding weights. After error correction of the fused coordinates, the optimal coordinates are obtained. The navigation control module calculates the target path point based on the optimal coordinates, obstacle recognition results, and preset weeding operation path, and controls the weeding robot to travel to the target path point.

2. The UWB navigation method based on visual feature fusion according to claim 1, characterized in that, When adaptively generating the layout of UWB anchor points, multiple populations are established. Different UWB anchor point layouts that satisfy the UWB anchor point parameter constraints are taken as individuals in each population. With the goal of minimizing the three-dimensional spatial geometric precision factor, selection, crossover, and mutation operations are iteratively performed on the individuals in each population to obtain the UWB anchor point layout corresponding to the optimal individual. The UWB anchor point parameter constraints include: the communication radius is a specified value, the installation height is the sum of the fruit tree height and a specified height, and the distance from the fruit tree trunk is greater than a distance threshold.

3. The UWB navigation method based on visual feature fusion according to claim 1, characterized in that, The orchard images include RGB images and near-infrared images. The weeding robot is also equipped with a light sensor for real-time acquisition of light values. When the visual perception module acquires orchard images in real time and identifies fruit trees and obstacles, it includes the following steps: If the illumination value is greater than the illumination threshold, the current RGB image is converted to HSV space, and overexposed areas are filtered out using a preset V channel threshold. Then, the processed current RGB image and the current near-infrared image are input into the trained fruit tree detection model and obstacle detection model. Otherwise, the current RGB image and the current near-infrared image are input into the trained fruit tree detection model and obstacle detection model. The fruit tree detection model extracts features from RGB and near-infrared images, then fuses these features using an attention mechanism. Based on the fused features, it identifies the fruit trees and finally obtains the trunk coordinates and calculates the center lines of the rows and columns. The obstacle detection model uses semantic segmentation on the RGB and near-infrared images to obtain an obstacle semantic map. The feature formula for fusing the RGB and near-infrared images using the attention mechanism is as follows: in, For the Sigmoid function, and Features of RGB and near-infrared images, respectively. and These are the fusion weights corresponding to the features of the RGB image and the near-infrared image, respectively.

4. The UWB navigation method based on visual feature fusion according to claim 3, characterized in that, When correcting visual coordinates, the following steps are included: Select keyframes from each orchard image at specified intervals; Calculate the deviation between the row and column spacing of fruit trees in the current frame of the orchard image and the row and column spacing of fruit trees in the corresponding key frame. If the deviation is less than the deviation threshold, calculate the difference between the visual coordinates of the current frame of the orchard image and the corresponding key frame to obtain the error correction amount. Use the error correction amount to correct the visual coordinates of the current frame of the orchard image and the subsequent frame of the orchard image.

5. The UWB navigation method based on visual feature fusion according to claim 4, characterized in that, When the data fusion processing module calculates the weights of UWB positioning data and visual positioning data, it includes the following steps: The formulas for calculating the UWB evaluation factor and the visual evaluation factor are as follows: in, As a UWB evaluation factor, For visual assessment factors, It is the signal strength of the communication between the UWB tag and the UWB anchor point in the current frame. It is the fruit tree coverage rate in the current frame. It is the soil attenuation coefficient corresponding to the soil moisture in the current frame. It is the illumination interference factor corresponding to the illumination value of the current frame. It is the weed occlusion factor corresponding to the obstacle recognition result of the current frame. It is the matching rate between the orchard image in the current frame and the corresponding keyframe, calculated using the following formula: Specifically, the number of matching points is the total number of feature points that match the feature point set F of the current frame orchard image with the feature point set F of the corresponding key frame, and the total number of extracted points is the total number of feature points in the feature point set F of the previous frame orchard image. The weights of UWB positioning data and visual positioning data are calculated based on UWB evaluation factors and visual evaluation factors, using the following formula: in, Indicates the weight of UWB positioning data. This indicates the weight of the visual positioning data.

6. The UWB navigation method based on visual feature fusion according to claim 5, characterized in that, When calculating the fused coordinates based on UWB positioning data, visual positioning data, and their corresponding weights, the Kalman filter algorithm is used. The formula is as follows: in, This represents the state prediction vector for the current frame. This represents the state vector of the previous frame. This represents the velocity vector of the weeding robot in the previous frame. This represents the x-coordinate value in the UWB coordinates of the UWB positioning data. This represents the x-coordinate value in the visual positioning coordinates of the visual positioning data. Indicates the merged coordinates of the previous frame. Indicates the gain of the current frame. This represents the covariance matrix of the error between the estimated state of the weeding robot and its actual state after the current frame state prediction is completed and before the measurement update. Here is the state transition matrix. For the control matrix, For the observation matrix, This represents the state vector of the current frame. The x and y components of the state vector are the values ​​of the x and y coordinates in the fused coordinate system. The fused coordinate observations for the current frame are given by the following formula: in, This represents UWB positioning data. This represents visual positioning data.

7. The UWB navigation method based on visual feature fusion according to claim 6, characterized in that, When correcting errors in the fused coordinates, the following steps are included: Calculate the vertical distance from the fused coordinates to the center line of the corresponding fruit tree row and column as the distance deviation. Calculate the distance from the fused coordinates to the nearest tree trunk in the orchard image. and according to the distance Calculate spacing deviation The formula is as follows: in, and These are the x and y coordinates from the merged coordinate system, respectively. Indicates the slope of the center line of the fruit tree rows. The intercept of the center line of the fruit tree row and column, This indicates the preset standard spacing between fruit trees; If distance deviation Spacing deviation If all values ​​are less than the corresponding threshold, the fused coordinates are taken as the optimal coordinates; otherwise, particle filter resampling is initiated to correct the fused coordinates and obtain the corresponding optimal coordinates.

8. The UWB navigation method based on visual feature fusion according to claim 1, characterized in that, The navigation control module calculates the target path point based on the optimal coordinates, obstacle recognition results, and the preset weeding operation path, and controls the weeding robot to travel to the target path point, including the following steps: The difference between the optimal coordinates of the current frame and the coordinates of the corresponding reference path point in the weeding operation path is calculated to obtain the path deviation. A cost map is constructed based on the obstacle recognition results. In the cost map, if no obstacle is recognized at the current position, the cost value is the minimum value; if an obstacle is recognized at the current position and the obstacle recognition result shows that the type is flexible or the diameter is less than the diameter threshold, the cost value is the intermediate value; if an obstacle is recognized at the current position and the obstacle recognition result shows that the type is rigid or the diameter is greater than the diameter threshold, the cost value is the maximum value. Using the optimal coordinates of the current frame as the starting point, calculate the heuristic function and cost function of the neighboring nodes around the starting point, and update the cost function using the cost value of the neighboring nodes in the cost map. Select the neighboring node with the smallest sum of the heuristic function and cost function as the target path point. The formula for the heuristic function is as follows: in, and These represent the coordinates of the current frame. and These are the x and y coordinates of the target path point, respectively. The mean y-axis value of the center lines of the rows and columns of fruit trees in the fruit tree identification results; To predict the future tilt angle, obtain the slope angle of the weeding robot's location and its current left and right wheel speeds, using the following formula: in, The slope angle, These are the current left wheel speed and right wheel speed, respectively. The PID parameters are corrected based on the slope angle and future tilt angle. Then, the PID controller with corrected parameters is used to calculate the motion control quantity of the weeding robot in the current frame, as shown in the following formula: in, For linear velocity in motion control quantities, Angular velocity is a variable in motion control. For the current actual linear velocity, The current actual angular velocity, , , These are the PID parameters for linear velocity; , , Angular velocity PID parameters; , The linear velocity and angular velocity deviations from the previous frame. , Let x be the linear velocity deviation and angular velocity deviation of the i-th frame. , The linear velocity deviation and angular velocity deviation for the current frame are calculated using the following formulas: in, and These represent the x-direction position deviation and the y-direction position deviation in the path deviation, respectively. and These are the proportionality coefficients corresponding to the linear velocity deviation and the angular velocity deviation, respectively. The desired left wheel speed and desired right wheel speed are calculated based on the linear velocity and linear velocity deviation in the motion control variables of the current frame of the weeding robot. The weeding robot is then controlled to travel to the target path point based on the desired left wheel speed and desired right wheel speed.

9. A UWB navigation system based on visual feature fusion, characterized in that, The system includes a UWB positioning module, a visual perception module, a data fusion processing module, and a navigation control module. The UWB positioning module includes UWB anchor points deployed in the orchard and UWB tags set in the weeding robot. The visual perception module, data fusion processing module, and navigation control module are all set in the weeding robot. The weeding robot is also equipped with a soil sensor. The UWB navigation system based on visual feature fusion is used to execute the steps of the UWB navigation method based on visual feature fusion as described in any one of claims 1 to 8.

Images